#include "sensors.h"
#include "esp_timer.h"
#include "radio.h"
#include "../hw/gps.h"
#include "servocontroller.h"

#define BLINK_GPIO 2

static uint8_t s_led_state = 0;

uint8_t telemetryIndex = 1;

uint8_t foundDevices[128];
uint8_t prevDevices[128];

static void configure_led(void)
{
    gpio_reset_pin(BLINK_GPIO);
    gpio_set_direction(BLINK_GPIO, GPIO_MODE_OUTPUT);
}

// void update_devices() {
//     memcpy(prevDevices, foundDevices, sizeof(prevDevices));
//     memset(foundDevices, 0, sizeof(foundDevices));
//     for (uint8_t i = 0; i < 128; i++)
//      {
//          fflush(stdout);
 
//          esp_err_t ret = i2c_master_probe(i2c0_bus_hdl, i, 20);
 
//          if (ret == ESP_OK)
//          {
//             foundDevices[i] = 1;
//              printf("Found device at 0x%02X\n", i);
//          }
//      }
// }

// void init_connected() {
//     for (uint8_t i = 0; i < 128; i++) {
//         if (foundDevices[i] != prevDevices[i]) {
//             if (foundDevices[i]) {
//                 switch (i)
//                 {
//                 case MCP23018_ADDRESS:
//                     /* code */
//                     if (mcp23018_init() == ESP_OK) {
//                         foundDevices[i] = 2;
//                     }
//                     break;

//                     case INA260_ADDRESS:
//                     if (ina260_init() == ESP_OK) {
//                         foundDevices[i] = 2;
//                     }
//                     /* code */
//                     break;

//                     case CCS811_ADDRESS:
//                     if (ccs811_init() == ESP_OK) {
//                         foundDevices[i] = 2;
//                     }
//                     /* code */
//                     break;

//                     case MPU9250_ADDRESS:
//                     if (mpu9250_init() == ESP_OK) {
//                         foundDevices[i] = 2;
//                     }
//                     /* code */
//                     break;

//                     case BME680_ADDRESS:
//                     if (bme680b_init() == ESP_OK) {
//                         foundDevices[i] = 2;
//                     }
//                     /* code */
//                     break;
                
//                 default:
//                     break;
//                 }
//             }
//         }
//     }
// }

void i2c_sensors_task(void *pvParameters)
{
    memset(foundDevices, 0, sizeof(foundDevices));
    memset(prevDevices, 0, sizeof(prevDevices));

    bme680b_init();
    mpu9250_init();
    ccs811_init();
    ina260_init();
    

    // update_devices();
    // init_connected();
    // initialize the xLastWakeTime variable with the current time.
    const int64_t interval_us = 100000; // 100 ms
    int64_t start_time, end_time, elapsed;

    //
    // initialize i2c device configuration

    mcp3550_spi_init();
    configure_led();

    int16_t accel[3], gyro[3], temp, magnet[3];
    float accel_f[3], gyro_f[3], temp_f, magnet_f[3];

    uint16_t eCO2;
    uint16_t tvoc;
    uint8_t currentCCS;
    uint16_t rawData;

    uint16_t volts;
    uint16_t current;
    uint16_t power;

    bme680_data_t bmeData;
    // task loop entry point
    for (;;)
    {
        start_time = esp_timer_get_time(); // µs since boot
        //
        // handle sensor

        if (BME680_DEV_HANDLE)
        {
            esp_err_t result = bme680_get_data(BME680_DEV_HANDLE, &bmeData);
            if (result != ESP_OK)
            {
                ESP_LOGE(TAG_BME, "bme680 device read failed (%s)", esp_err_to_name(result));
            }
            else
            {
                bmeData.barometric_pressure = bmeData.barometric_pressure / 100;
                ESP_LOGI(TAG_BME, "dewpoint temperature:%.2f °C", bmeData.dewpoint_temperature);
                ESP_LOGI(TAG_BME, "air temperature:     %.2f °C", bmeData.air_temperature);
                ESP_LOGI(TAG_BME, "relative humidity:   %.2f %%", bmeData.relative_humidity);
                ESP_LOGI(TAG_BME, "barometric pressure: %.2f hPa", bmeData.barometric_pressure);
                ccs811_set_env_data(bmeData.air_temperature, bmeData.relative_humidity);
                ESP_LOGI(TAG_BME, "gas resistance:      %.2f kOhms", bmeData.gas_resistance / 1000);
                ESP_LOGI(TAG_BME, "iaq score:           %u (%s)", bmeData.iaq_score, bme680_air_quality_to_string(bmeData.iaq_score));
            }
        }
        else
        {
            bme680b_init();
        }

        ccs811_get_data(&eCO2, &tvoc, &currentCCS, &rawData);
        ESP_LOGI(TAG_CCS, "eCO₂: %d ppm, TVOC: %d ppb", eCO2, tvoc);
        ESP_LOGI(TAG_CCS, "Current: %d μA, Raw voltage: %d V", currentCCS, rawData);

        if (mpu9250_read_sensor_data(MPU9250_DEV_HANDLE, accel, gyro, &temp, magnet) == ESP_OK)
        {
            mpu9250_convert_data(accel, gyro, temp, magnet, accel_f, gyro_f, &temp_f, magnet_f);

            ESP_LOGI(TAG_MPU, "Accel: X=%.2f g, Y=%.2f g, Z=%.2f g", accel_f[0], accel_f[1], accel_f[2]);
            ESP_LOGI(TAG_MPU, "Gyro: X=%.2f°/s, Y=%.2f°/s, Z=%.2f°/s", gyro_f[0], gyro_f[1], gyro_f[2]);
            ESP_LOGI(TAG_MPU, "Magnet: X=%.2fμT, Y=%.2fμT, Z=%.2fμT", magnet_f[0], magnet_f[1], magnet_f[2]);
            ESP_LOGI(TAG_MPU, "Temperature: %.2f °C", temp_f);
        }
        else
        {
            ESP_LOGE(TAG_MPU, "Failed to read sensor data");
        }

        ina260_readParams(&volts, &current, &power);
        ina260_printParams(volts, current, power);

        float VREFVoltage = 2.5;

        // mics_adc_data_t ADCData;
        // memset(&ADCData, 0, sizeof(ADCData));
        mics_adc_data_t ADCData = mcp3550_read_all(VREFVoltage);

        log_mics_adc_values(&ADCData);

        //int32_t nh3val = mcp3550_read(MCP_CS_ADC_NH3);

        //ESP_LOGI(TAG_BME, "MICS NH3: %ld -> %fV", nh3val, mcp3550_to_voltage(nh3val, VREFVoltage));

        //gpio_set_level(BLINK_GPIO, s_led_state);
        mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CS_ADC_UVC, s_led_state);
        /* Toggle the LED state */
        s_led_state = !s_led_state;
        end_time = esp_timer_get_time();
        elapsed = end_time - start_time;

        if (elapsed < interval_us)
        {
            vTaskDelay(pdMS_TO_TICKS((interval_us - elapsed) / 1000));
        }
        if (packetReadiness == 0) {
            memset(&telemetryPacket, 0, sizeof(telemetryPacket));
            telemetryPacket.accelerationX = accel[0];
            telemetryPacket.accelerationY = accel[1];
            telemetryPacket.accelerationZ = accel[2];

            telemetryPacket.gyroX = gyro[0];
            telemetryPacket.gyroX = gyro[1];
            telemetryPacket.gyroX = gyro[2];

            telemetryPacket.magnetX = gyro[0];
            telemetryPacket.magnetX = gyro[1];
            telemetryPacket.magnetX = gyro[2];

            telemetryPacket.accelerometer_temperature = temp;

            telemetryPacket.eCO2 = eCO2;
            telemetryPacket.tvoc = tvoc;
            telemetryPacket.currentCCS = currentCCS;
            telemetryPacket.rawCCSData = rawData;

            telemetryPacket.volts = volts;
            telemetryPacket.current = current;
            telemetryPacket.power = power;


            telemetryPacket.temperature = bmeData.raw_data.temperature;
            telemetryPacket.humidity = bmeData.raw_data.humidity;
            telemetryPacket.pressure = bmeData.raw_data.pressure;
            telemetryPacket.gas = bmeData.raw_data.gas;
            telemetryPacket.gas_range = bmeData.raw_data.gas_range;
            telemetryPacket.heater_stable = bmeData.raw_data.heater_stable;
            telemetryPacket.gas_valid = bmeData.raw_data.gas_valid;

            telemetryPacket.air_temperature = bmeData.air_temperature;
            telemetryPacket.relative_humidity = bmeData.relative_humidity;
            telemetryPacket.barometric_pressure = bmeData.barometric_pressure;
            telemetryPacket.gas_resistance = bmeData.gas_resistance;
            telemetryPacket.iaq_score = bmeData.iaq_score;
            telemetryPacket.temperature_score = bmeData.temperature_score;
            telemetryPacket.humidity_score = bmeData.humidity_score;
            telemetryPacket.gas_score = bmeData.gas_score;

            telemetryPacket.NH3 = ADCData.raw_nh3;
            telemetryPacket.CO = ADCData.raw_co;
            telemetryPacket.NO2 = ADCData.raw_no2;
            telemetryPacket.UVC = ADCData.raw_uvc;

            //TODO MOVE THIS TO A BETTER PLACE FOR SYNC
            telemetryPacket.time_seconds = gpsDataOut.time_seconds;
            telemetryPacket.latitude_centi_degrees = gpsDataOut.latitude_centi_degrees;
            telemetryPacket.longitude_centi_degrees = gpsDataOut.longitude_centi_degrees;
            telemetryPacket.fix_quality = gpsDataOut.fix_quality;
            telemetryPacket.num_satellites = gpsDataOut.num_satellites;
            telemetryPacket.altitude_centi_meters = gpsDataOut.altitude_centi_meters;
            telemetryPacket.date_yyddmm = gpsDataOut.date_yyddmm;
            telemetryPacket.speed_centi_knots = gpsDataOut.speed_centi_knots;

            telemetryPacket.predicted_altitude_centi_meters = predictedPosition.altitude_centi_meters;
            telemetryPacket.predicted_latitude_centi_degrees = predictedPosition.latitude_centi_degrees;
            telemetryPacket.predicted_longitude_centi_degrees = predictedPosition.longitude_centi_degrees;

            telemetryPacket.targetServoA = servoState.targetServoA;
            telemetryPacket.targetServoB = servoState.targetServoB;
            telemetryPacket.currentServoA = servoState.currentServoA;
            telemetryPacket.currentServoB = servoState.currentServoB;

            telemetryPacket.telemetryIndex = telemetryIndex++;


        }
        packetReadiness = 1;
    }
    //
    // free resources
    bme680_delete(BME680_DEV_HANDLE);
    vTaskDelete(NULL);
}