#include "dht20.h"
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <cstring>

#define TAG "DHT20"

// Timing constants (in milliseconds)
#define DHT20_RESET_DELAY_MS 300
#define DHT20_INIT_DELAY_MS 500
#define DHT20_MEASUREMENT_DELAY_MS 80
#define DHT20_READ_TIMEOUT_MS 100

DHT20::DHT20(i2c_master_bus_handle_t i2c_bus)
    : I2cDevice(i2c_bus, DHT20_I2C_ADDR) {
}

esp_err_t DHT20::Initialize() {
    ESP_LOGI(TAG, "Initializing DHT20 sensor...");

    // Step 1: Reset sensor
    esp_err_t ret = Reset();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to reset sensor: %s", esp_err_to_name(ret));
        return ret;
    }
    vTaskDelay(pdMS_TO_TICKS(DHT20_RESET_DELAY_MS));

    // Step 2: Set system configuration
    ret = SetSystemConfig();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to set system config: %s", esp_err_to_name(ret));
        return ret;
    }
    vTaskDelay(pdMS_TO_TICKS(DHT20_INIT_DELAY_MS));

    // Step 3: Verify calibration status (retry up to 5 times)
    int retry_count = 0;
    while (retry_count < 5) {
        if (IsCalibrated()) {
            ESP_LOGI(TAG, "DHT20 initialized and calibrated successfully");
            return ESP_OK;
        }

        ESP_LOGW(TAG, "Sensor not calibrated, retrying... (%d/5)", retry_count + 1);

        // Retry: reset and reconfigure
        Reset();
        vTaskDelay(pdMS_TO_TICKS(DHT20_RESET_DELAY_MS));
        SetSystemConfig();
        vTaskDelay(pdMS_TO_TICKS(DHT20_INIT_DELAY_MS));

        retry_count++;
    }

    ESP_LOGE(TAG, "Failed to calibrate sensor after %d retries", retry_count);
    return ESP_FAIL;
}

esp_err_t DHT20::ReadTempAndHumidity(float& temperature, float& humidity) {
    // Step 1: Check if sensor is busy
    uint8_t status = ReadStatus();
    if (status & DHT20_STATUS_BUSY_BIT) {
        ESP_LOGW(TAG, "Sensor is busy, waiting...");
        vTaskDelay(pdMS_TO_TICKS(DHT20_MEASUREMENT_DELAY_MS));
    }

    // Step 2: Trigger measurement
    esp_err_t ret = TriggerMeasurement();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to trigger measurement: %s", esp_err_to_name(ret));
        return ret;
    }

    // Step 3: Wait for measurement to complete
    vTaskDelay(pdMS_TO_TICKS(DHT20_MEASUREMENT_DELAY_MS));

    // Step 4: Read sensor data (7 bytes)
    uint8_t data[7] = {0};
    ret = ReadData(data, sizeof(data));
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read sensor data: %s", esp_err_to_name(ret));
        return ret;
    }

    // Step 5: Check if measurement is complete (status bit[7] should be 0)
    if (data[0] & DHT20_STATUS_BUSY_BIT) {
        ESP_LOGE(TAG, "Measurement not complete (status=0x%02X)", data[0]);
        return ESP_ERR_TIMEOUT;
    }

    // Step 6: Parse humidity data (20 bits)
    // Humidity = [Byte1<<12 | Byte2<<4 | Byte3>>4] × 100 / 2^20
    uint32_t humidity_raw = 0;
    humidity_raw = ((uint32_t)data[1] << 12) | ((uint32_t)data[2] << 4) | ((uint32_t)data[3] >> 4);
    humidity = (float)humidity_raw * 100.0f / 1048576.0f; // 2^20 = 1048576

    // Step 7: Parse temperature data (20 bits)
    // Temperature = [(Byte3&0x0F)<<16 | Byte4<<8 | Byte5] × 200 / 2^20 - 50
    uint32_t temperature_raw = 0;
    temperature_raw = (((uint32_t)data[3] & 0x0F) << 16) | ((uint32_t)data[4] << 8) | (uint32_t)data[5];
    temperature = (float)temperature_raw * 200.0f / 1048576.0f - 50.0f;

    ESP_LOGD(TAG, "Temperature: %.2f°C, Humidity: %.2f%%", temperature, humidity);

    // Optional: CRC check (data[6]) - currently not implemented
    // This could be added for extra reliability if needed

    return ESP_OK;
}

bool DHT20::IsCalibrated() {
    uint8_t status = ReadStatus();
    bool calibrated = (status & DHT20_STATUS_CALIBRATED_BIT) != 0;
    ESP_LOGD(TAG, "Calibration status: 0x%02X (calibrated=%d)", status, calibrated);
    return calibrated;
}

esp_err_t DHT20::Reset() {
    ESP_LOGD(TAG, "Resetting sensor...");
    return WriteByte(DHT20_RESET_REG);
}

esp_err_t DHT20::SetSystemConfig() {
    ESP_LOGD(TAG, "Setting system configuration...");
    const uint8_t config_data[] = {DHT20_CALIBRATE_REG, 0x08, 0x00};
    return WriteBytes(config_data, sizeof(config_data));
}

uint8_t DHT20::ReadStatus() {
    uint8_t status = 0;
    esp_err_t ret = i2c_master_receive(i2c_device_, &status, 1, DHT20_READ_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read status: %s", esp_err_to_name(ret));
        return 0xFF; // Return invalid status on error
    }
    return status;
}

esp_err_t DHT20::TriggerMeasurement() {
    ESP_LOGD(TAG, "Triggering measurement...");
    const uint8_t trigger_cmd[] = {0xAC, 0x33, 0x00};
    return WriteBytes(trigger_cmd, sizeof(trigger_cmd));
}

esp_err_t DHT20::ReadData(uint8_t* buffer, size_t len) {
    if (buffer == nullptr || len == 0) {
        return ESP_ERR_INVALID_ARG;
    }

    esp_err_t ret = i2c_master_receive(i2c_device_, buffer, len, DHT20_READ_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read data: %s", esp_err_to_name(ret));
        return ret;
    }

    ESP_LOGD(TAG, "Read %d bytes: %02X %02X %02X %02X %02X %02X %02X",
             len, buffer[0], buffer[1], buffer[2], buffer[3],
             buffer[4], buffer[5], buffer[6]);

    return ESP_OK;
}

esp_err_t DHT20::WriteBytes(const uint8_t* data, size_t len) {
    if (data == nullptr || len == 0) {
        return ESP_ERR_INVALID_ARG;
    }

    esp_err_t ret = i2c_master_transmit(i2c_device_, data, len, DHT20_READ_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to write %d bytes: %s", len, esp_err_to_name(ret));
        return ret;
    }

    return ESP_OK;
}

esp_err_t DHT20::WriteByte(uint8_t byte) {
    return WriteBytes(&byte, 1);
}