#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <driver/i2c.h>
#include <stdio.h>
#include <esp_log.h>
#include "functions.h"
#include "sht31.h"

#define SDA_PIN GPIO_NUM_14
#define CLK_PIN GPIO_NUM_13
#define SENSOR_ADDR 0x44 //default address is 0x44 (pin 2 logic low). We chose 0x45 with logic high in v0.9 and 0x44 with newer versions where second sensor is removed.
#define SHT31_MEASURE_CMD_HIGHREP 0x2400
#define SHT31_HEATER_OFF_CMD 0x3066

static const char * TAG = "sht31.c";

// Dummy implementations for the purpose of this example
// Replace these with actual sensor initialization and data retrieval logic

static const i2c_config_t config = {
    .mode             = I2C_MODE_MASTER,
    .sda_io_num       = SDA_PIN,
    .scl_io_num       = CLK_PIN,
    .sda_pullup_en    = GPIO_PULLUP_DISABLE,
    .scl_pullup_en    = GPIO_PULLUP_DISABLE,
    .clk_stretch_tick = 5
};

void sht31_sensor_test_task(void *pvParameters)
{
    ESP_ERROR_CHECK(sht31_init());

    //float temperature, humidity;
    float temperature, humidity, temp_dec_part, hum_dec_part;
    int temp_int_part, hum_int_part;

    while (1) {
        if (sht31_read_temp_hum(&temperature, &humidity) == ESP_OK) {
	    split_float(temperature, &temp_int_part, &temp_dec_part);
	    split_float(humidity, &hum_int_part, &hum_dec_part);
            //ESP_LOGI(TAG, "Temperature: %.2f°C, Humidity: %.2f%%", temperature, humidity);
            ESP_LOGI(TAG, "Temperature: %i.%i°C, Humidity: %i.%i%%", temp_int_part, (int)(temp_dec_part*1000), hum_int_part, (int)(hum_dec_part*1000));
        } else {
            ESP_LOGE(TAG, "Failed to read from SHT31 sensor");
        }
        vTaskDelay(30000 / portTICK_RATE_MS);
    }
}

// Function to initialize the sensor
esp_err_t sht31_init(void) {
    esp_err_t install_rc = i2c_driver_install(I2C_NUM_0, config.mode);
    esp_err_t config_rc  = i2c_param_config(I2C_NUM_0, &config);
    if( install_rc != ESP_OK || config_rc != ESP_OK ) {
        printf("Sensor initialization failed!\n");
        return ESP_FAIL;
    }

    // Send the heater off command
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (SENSOR_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, SHT31_HEATER_OFF_CMD >> 8, true);
    i2c_master_write_byte(cmd, SHT31_HEATER_OFF_CMD & 0xFF, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send command to SHT31");
        return ret;
    }



    printf("Sensor initialized.\n");
    return ESP_OK;
}

esp_err_t sht31_read_temp_hum(float *temperature, float *humidity)
{
    uint8_t data[6];
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();

    // Send the measurement command
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (SENSOR_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, SHT31_MEASURE_CMD_HIGHREP >> 8, true);
    i2c_master_write_byte(cmd, SHT31_MEASURE_CMD_HIGHREP & 0xFF, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send command to SHT31");
        return ret;
    }

    vTaskDelay(500 / portTICK_RATE_MS);

    // Request 6 bytes of data from the sensor
    cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (SENSOR_ADDR << 1) | I2C_MASTER_READ, true);
    i2c_master_read     (cmd, data,  sizeof(data) - 1, I2C_MASTER_ACK);
    i2c_master_read_byte(cmd, data + sizeof(data) - 1, I2C_MASTER_NACK);
    i2c_master_stop(cmd);
    ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read data from SHT31");
        return ret;
    }

    // Process the data
    uint16_t t = (data[0] << 8) | data[1];
    uint16_t h = (data[3] << 8) | data[4];

    *temperature = -45 + 175 * ((float)t / 65535.0);
    *humidity = 100 * ((float)h / 65535.0);

    return ESP_OK;
}