#pragma once

#include <Arduino.h>

#include <SensirionI2CScd4x.h>
#include <Wire.h>

#include "data.h"

class CO2 {
private:
  SensirionI2CScd4x scd4x;

public:
  CO2() : scd4x() {}

  void start() {
    uint16_t error;
    char errorMessage[256];

    scd4x.begin(Wire);

    // stop potentially previously started measurement
    error = scd4x.stopPeriodicMeasurement();
    if (error) {
        Serial.print("! CO2 stopPeriodicMeasurement() error: ");
        errorToString(error, errorMessage, 256);
        Serial.println(errorMessage);
    }

    error = scd4x.setAutomaticSelfCalibration(1);
    if (error) {
        Serial.print("! CO2 setAutomaticSelfCalibration() error: ");
        errorToString(error, errorMessage, 256);
        Serial.println(errorMessage);
    }
    
    // Start Measurement
    error = scd4x.startLowPowerPeriodicMeasurement();
    if (error) {
        Serial.print("! CO2 startLowPowerPeriodicMeasurement() error: ");
        errorToString(error, errorMessage, 256);
        Serial.println(errorMessage);
    }
  }

  bool dataReady() {
    uint16_t ready;
    uint16_t error;
    char errorMessage[256];

    error = scd4x.getDataReadyStatus(ready);
    if (error) {
        Serial.print("! CO2 getDataReadyStatus() error: ");
        errorToString(error, errorMessage, 256);
        Serial.println(errorMessage);
        return false;
    }

    // "If last 11 bits are 0 data not ready, else data ready"
    return (ready & 0x7FFF) != 0;
  }
  
  void read(SensorData *data) {
    uint16_t error;
    char errorMessage[256];

    error = scd4x.readMeasurement(data->co2, data->temperature, data->humidity);
    if (error) {
        Serial.print("! CO2 readMeasurement() error: ");
        errorToString(error, errorMessage, 256);
        Serial.println(errorMessage);
    }
  }

  bool serialCommand(const String &tag, const String &command) {
    if (command.startsWith("setco ")) {
      uint16_t error;
      char errorMessage[256];

      uint16_t target = command.substring(6).toInt();
      uint16_t correction;

      scd4x.stopPeriodicMeasurement();
      delay(500);
      error = scd4x.performForcedRecalibration(target, correction);
      if (error) {
        Serial.print("! CO2 performForcedRecalibration() error: ");
        errorToString(error, errorMessage, 256);
        Serial.println(errorMessage);
      }

      Serial.print(tag);Serial.print(" setco ");
      if (correction == 0xFFFF) {
        Serial.println("failed");
      }
      else {
        Serial.println(correction - 0x8000);
      }

      error = scd4x.startLowPowerPeriodicMeasurement();
      if (error) {
        Serial.print("! CO2 startLowPowerPeriodicMeasurement() error: ");
        errorToString(error, errorMessage, 256);
        Serial.println(errorMessage);
      }

      return true;
    }

    return false;
  }
};