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

#include <sys/unistd.h>
#include <sys/stat.h>
#include "esp_vfs_fat.h"
#include "sdmmc_cmd.h"

#define MOUNT_POINT "/sdcard"
#define MAX_LINE_LENGTH 64

#define BLINK_GPIO 2

// uint8_t powerMode = LOW_POWER_MODE;
uint8_t powerMode = HIGH_POWER_MODE;

static uint8_t s_led_state = 0;

uint8_t telemetryIndex = 1;

const uint8_t expectedAdressesCount = 5;
const uint8_t expectedAdresses[] = {MCP23018_ADDRESS, BME680_ADDRESS, CCS811_ADDRESS, MPU9250_ADDRESS, INA260_ADDRESS};

uint8_t foundDevices[5];
uint8_t prevDevices[5];

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));
    for (uint8_t i = 0; i < expectedAdressesCount; i++)
    {
        fflush(stdout);

        esp_err_t ret = i2c_master_probe(i2c0_bus_hdl, expectedAdresses[i], 20);

        if (ret == ESP_OK)
        {
            if (foundDevices[i] == 0)
            {
                foundDevices[i] = 1;
            }
            // printf("Found device at 0x%02X\n", i);
        }
        else
        {
            foundDevices[i] = 0;
            if (i == 1 && powerMode != HIGH_POWER_MODE)
            {
                continue;
            }
            printf("Not found device at 0x%02X\n", expectedAdresses[i]);
        }
    }
}

void setPowerMode(uint8_t powerModeIn)
{
    powerMode = powerModeIn;
    if (foundDevices[0] == 2)
    {
        if (powerMode == HIGH_POWER_MODE)
        {
            mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CCS811_WAKE, 0);
            mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CCS811_POWER, 1);
            mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_MICS_POWER, 1);
        }
        else
        {
            mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CCS811_WAKE, 1);
            mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_CCS811_POWER, 0);
            mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_MICS_POWER, 0);
        }
    }
}

void init_connected()
{

    for (uint8_t i = 0; i < expectedAdressesCount; i++)
    {
        if (foundDevices[i] != prevDevices[i])
        {
            if (foundDevices[i])
            {
                esp_err_t ret = ESP_FAIL;
                bool foundUsed = true;
                switch (i)
                {
                case MCP23018_ADDRESS:
                    /* code */
                    ret = mcp23018_init();
                    setPowerMode(powerMode);
                    mcp3550_spi_init();
                    break;

                case INA260_ADDRESS:
                    ret = ina260_init();
                    /* code */
                    break;

                case CCS811_ADDRESS:
                    ret = ccs811_init();
                    /* code */
                    break;

                case MPU9250_ADDRESS:
                    ret = mpu9250_init();
                    /* code */
                    break;

                case BME680_ADDRESS:
                    ret = bme680b_init();
                    /* code */
                    break;

                default:
                    foundUsed = false;
                    break;
                }
                if (foundUsed)
                {
                    if (ret == ESP_OK)
                    {
                        foundDevices[i] = 2;
                    }
                    else
                    {
                        printf("Device init error at 0x%02X - %s\n", expectedAdresses[i], esp_err_to_name(ret));
                    }
                }
            }
        }
    }
}

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 = 50000; // 50 ms
    int64_t start_time, end_time, elapsed;

    //
    // initialize i2c device configuration

    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;

    mics_adc_data_t ADCData;
    // task loop entry point
    for (;;)
    {
        start_time = esp_timer_get_time(); // µs since boot

        uint8_t presentDevices = 0xFF;
        // update_devices();
        // init_connected();

        //
        // handle sensor

        presentDevices |= BME680_PRESENT_BIT;

        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();
        }

        presentDevices |= CCS811_PRESENT_BIT;
        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);

        presentDevices |= MPU9250_PRESENT_BIT;
        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");
        }

        presentDevices |= INA260_PRESENT_BIT;
        ina260_readParams(&volts, &current, &power);
        ina260_printParams(volts, current, power);

        presentDevices |= MCP23018_PRESENT_BIT;
        float VREFVoltage = 2.5;
        ADCData = mcp3550_read_all(VREFVoltage);

        log_mics_adc_values(&ADCData);

        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.presentDevices = presentDevices;

            telemetryPacket.telemetryIndex = telemetryIndex++;
        }
        packetReadiness = 1;

        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));
        }
    }
    //
    // free resources
    bme680_delete(BME680_DEV_HANDLE);
    vTaskDelete(NULL);
}