#include "scd30.h"
#include <string.h>
#include <stdio.h>

#define SCD30_I2C_ADDRESS 0x61
#define SCD30_CMD_START_MEASUREMENT 0x0010
#define SCD30_CMD_READ_DATA 0x0300
#define SCD30_CMD_READY_STATUS 0x0202

// Private function to calculate CRC-8
static uint8_t calculate_crc8(const uint8_t *data, size_t len) {
    const uint8_t polynomial = 0x31;
    uint8_t crc = 0xFF;

    for (size_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t bit = 0; bit < 8; bit++) {
            if (crc & 0x80) {
                crc = (crc << 1) ^ polynomial;
            } else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

scd30_handle_t *scd30_init(int sda_gpio, int scl_gpio) {
    static scd30_handle_t handle;
    i2c_master_bus_config_t bus_config = {
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .i2c_port = I2C_NUM_0,
        .scl_io_num = scl_gpio,
        .sda_io_num = sda_gpio,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true
    };
    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, &handle.bus_handle));

    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = SCD30_I2C_ADDRESS,
        .scl_speed_hz = 100000,
        .scl_wait_us = 150000 // Clock stretching
    };
    ESP_ERROR_CHECK(i2c_master_bus_add_device(handle.bus_handle, &dev_cfg, &handle.dev_handle));
    return &handle;
}

void scd30_start_measurement(scd30_handle_t *handle) {
    uint8_t command[5] = {
        (SCD30_CMD_START_MEASUREMENT >> 8) & 0xFF,
        SCD30_CMD_START_MEASUREMENT & 0xFF,
        0x00, 0x10, // Argument for continuous measurement
        0x00 // Placeholder for CRC
    };
    command[4] = calculate_crc8(&command[2], 2);
    i2c_master_transmit(handle->dev_handle, command, sizeof(command), 1000);
}

// Helper to convert Big Endian IEEE754 bytes to float
static float bytes_to_float(uint8_t mmsb, uint8_t mlsb, uint8_t lmsb, uint8_t llsb) {
    uint32_t val = ((uint32_t)mmsb << 24) | ((uint32_t)mlsb << 16) | ((uint32_t)lmsb << 8) | (uint32_t)llsb;
    float f;
    memcpy(&f, &val, 4);
    return f;
}

int scd30_read_data(scd30_handle_t *handle, scd30_data_t *data) {
    uint8_t ready_command[2] = {
        (SCD30_CMD_READY_STATUS >> 8) & 0xFF,
        SCD30_CMD_READY_STATUS & 0xFF
    };
    uint8_t ready_status[3];

    // Poll until data is ready
    do {
        i2c_master_transmit_receive(handle->dev_handle, ready_command, sizeof(ready_command), ready_status, sizeof(ready_status), 1000);
    } while (ready_status[1] != 0x01);

    uint8_t read_command[2] = {
        (SCD30_CMD_READ_DATA >> 8) & 0xFF,
        SCD30_CMD_READ_DATA & 0xFF
    };
    uint8_t buffer[18];

    // Read 18 bytes of data
    // Format: CO2_MSB, CO2_LSB, CRC, CO2_LMSB, CO2_LLSB, CRC, ...
    i2c_master_transmit_receive(handle->dev_handle, read_command, sizeof(read_command), buffer, sizeof(buffer), 1000);

    // Validate CRC for each pair of data
    for (int i = 0; i < 18; i += 3) {
        if (calculate_crc8(buffer + i, 2) != buffer[i + 2]) {
            return -1; // CRC error
        }
    }

    // Parse IEEE754 floats (Big Endian)
    // Structure: [Byte1, Byte2, CRC, Byte3, Byte4, CRC]
    //            0      1      2    3      4      5
    
    data->co2 = bytes_to_float(buffer[0], buffer[1], buffer[3], buffer[4]);
    data->temp = bytes_to_float(buffer[6], buffer[7], buffer[9], buffer[10]);
    data->humidity = bytes_to_float(buffer[12], buffer[13], buffer[15], buffer[16]);

    return 0; // Success
}