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


void Sensor::begin() {
  Wire.begin();
  scd30.begin();
}

bool Sensor::canRun(uint32_t now) {
  if (now > runTime) {
    runTime += 500;
    return scd30.dataAvailable();
  }
  else
    return 0;
}

void Sensor::run(uint32_t now) {
  uint16_t co2;
  co2 = scd30.getCO2();
  Serial.println(co2, DEC);
  if (calibrating) {
    indicator->off();
    // Check whether the measurements have stabilized.
    // Sample NB_VALUES, calculate the absolute value of the difference from
    // each value to the previous one.

    values[value_idx] = co2;
    uint16_t diff_abs_errors_sum = 0;
    for(uint8_t i=0; i < NB_VALUES-1; i++) {
      int16_t d = values[(value_idx + NB_VALUES - i) % NB_VALUES] - values[(value_idx + NB_VALUES - i - 1) % NB_VALUES];
      diff_abs_errors_sum += abs(d);
    }
    // Calculate the difference between the current sample and NB_VALUES
    // samples ago.
    int16_t d = values[value_idx] - values[(value_idx + 1) % NB_VALUES];
    uint16_t abs_drop = abs(d);
    // Ideally, diff_abs_errors_sum should be zero, but we allow each absolute
    // difference to be at most 1 (and therefore the sum to be at most NB_VALUES),
    // to account for a bit of noise. We also allow the change over the
    // sampling period to be at most 1, meaning that the value can oscillate
    // betwen N and N+1, which is a stable situation with a bit of noise.
    if (abs_drop <= 1 && diff_abs_errors_sum <= NB_VALUES) {
      // The measured value has been stable for a while, so we calibrate the sensor
      Serial.println("CALIBRATE");
      scd30.setForcedRecalibrationFactor(REFERENCE_CO2_LEVEL);
      delay(10);
      calibrating = false;
      indicator->update(REFERENCE_CO2_LEVEL);
      for(uint8_t i=0; i < NB_VALUES; i++) values[i] = 0;
      value_idx = 0;
    }
    else {
      value_idx = (value_idx + 1) % NB_VALUES;
    }
  }
  else {
    indicator->update(co2);
  }
}

// vim:ft=arduino