#include "scd30.h"
#include "driver/i2c_master.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;
}

void scd30_init(void) {
    i2c_master_config_t i2c_config = {
        .scl_wait_us = 150000 // Clock stretching
    };
    i2c_master_init(I2C_NUM_0, &i2c_config);
}

void scd30_start_measurement(void) {
    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_write_to_device(I2C_NUM_0, SCD30_I2C_ADDRESS, command, sizeof(command), 1000 / portTICK_PERIOD_MS);
}

int scd30_read_data(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_write_read_device(I2C_NUM_0, SCD30_I2C_ADDRESS, ready_command, sizeof(ready_command), ready_status, sizeof(ready_status), 1000 / portTICK_PERIOD_MS);
    } 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
    i2c_master_write_read_device(I2C_NUM_0, SCD30_I2C_ADDRESS, read_command, sizeof(read_command), buffer, sizeof(buffer), 1000 / portTICK_PERIOD_MS);

    // 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 data
    data->co2 = (float)((buffer[0] << 8) | buffer[1]);
    data->temp = (float)((buffer[3] << 8) | buffer[4]);
    data->humidity = (float)((buffer[6] << 8) | buffer[7]);

    return 0; // Success
}