#include "sensor.h"
#include <Wire.h>

// TODO Trigger a reset when an error occurs
void handle_error(int code, char *msg) {
    if (code) {
        errorToString(code, msg, sizeof msg);
        exit(1);
    }
}

void initialize_sensor(SensirionI2cScd4x &sensor, int error_code,
                       char *error_msg) {
    Wire.begin();
    sensor.begin(Wire, SCD41_I2C_ADDR_62);
    Serial.println("Starting sensor initialization");

    error_code = sensor.wakeUp();
    handle_error(error_code, error_msg);
    error_code = sensor.stopPeriodicMeasurement();
    handle_error(error_code, error_msg);
    error_code = sensor.reinit();
    handle_error(error_code, error_msg);
    error_code = sensor.startPeriodicMeasurement();
    handle_error(error_code, error_msg);

    Serial.println("The sensor was initialized properly");
};

void read_values(SensirionI2cScd4x &sensor, float *data, int error_code,
                 char *error_msg) {
    bool data_available = false;
    short unsigned int co2_concentration = 0.0;

    error_code = sensor.getDataReadyStatus(data_available);
    handle_error(error_code, error_msg);

    while (!data_available) {
        delay(100);
        error_code = sensor.getDataReadyStatus(data_available);
        handle_error(error_code, error_msg);
    }

    Serial.println("Data fetched from the sensor");

    error_code = sensor.readMeasurement(co2_concentration, data[1], data[2]);
    data[0] = co2_concentration;
    handle_error(error_code, error_msg);
}
