#include "co2_sensor.h"

namespace config {
  // Values should be defined in config.h
  uint16_t measurement_timestep = MEASUREMENT_TIMESTEP; // [s] Value between 2 and 1800 (range for SCD30 sensor)
  const uint16_t altitude_above_sea_level = ALTITUDE_ABOVE_SEA_LEVEL; // [m]
  uint16_t co2_calibration_level = ATMOSPHERIC_CO2_CONCENTRATION; // [ppm]
  int8_t max_deviation_during_calibration = 30; // [ppm]
  int8_t enough_stable_measurements = 60;
#ifdef TEMPERATURE_OFFSET
  // Residual heat from CO2 sensor seems to be high enough to change the temperature reading. How much should it be offset?
  // NOTE: Sign isn't relevant. The returned temperature will always be shifted down.
  const float temperature_offset = TEMPERATURE_OFFSET; // [K]
#else
  const float temperature_offset = -3.0;  // [K] Temperature measured by sensor is usually at least 3K too high.
#endif
  bool auto_calibrate_sensor = AUTO_CALIBRATE_SENSOR; // [true / false]
  const bool debug_sensor_states = false; // If true, log state transitions over serial console
}

namespace sensor {
  SCD30 scd30;
  uint16_t co2 = 0;
  float temperature = 0;
  float humidity = 0;
  char timestamp[23];
  int16_t stable_measurements = 0;

  /**
   * Define sensor states
   * INITIAL -> initial state
   * BOOTUP -> state after initializing the sensor, i.e. after scd.begin()
   * READY -> sensor does output valid information (> 0 ppm) and no other condition takes place
   * NEEDS_CALIBRATION -> sensor measurements are too low (< 250 ppm)
   * PREPARE_CALIBRATION -> forced calibration was initiated, waiting for stable measurements
   * CALIBRATION -> the sensor does calibrate itself
   */
  enum state {
    INITIAL,
    BOOTUP,
    READY,
    NEEDS_CALIBRATION,
    PREPARE_CALIBRATION_UNSTABLE,
    PREPARE_CALIBRATION_STABLE,
    CALIBRATION
  };
  const char *state_names[] = {
      "INITIAL",
      "BOOTUP",
      "READY",
      "NEEDS_CALIBRATION",
      "PREPARE_CALIBRATION_UNSTABLE",
      "PREPARE_CALIBRATION_STABLE",
      "CALIBRATION" };
  state current_state = INITIAL;
  void switchState(state);

  void initialize() {
#if defined(ESP8266)
    Wire.begin(12, 14); // ESP8266 - D6, D5;
#endif
#if defined(ESP32)
    Wire.begin(21, 22); // ESP32
    /**
     *  SCD30   ESP32
     *  VCC --- 3V3
     *  GND --- GND
     *  SCL --- SCL (GPIO22) //NOTE: GPIO3 Would be more convenient (right next to GND)
     *  SDA --- SDA (GPIO21) //NOTE: GPIO1 would be more convenient (right next to GPO3)
     */
#endif

    // CO2
    if (scd30.begin(config::auto_calibrate_sensor) == false) {
      Serial.println(F("ERROR - CO2 sensor not detected. Please check wiring!"));
      led_effects::showKITTWheel(color::red, 30);
      ESP.restart();
    }

    switchState(BOOTUP);

    // SCD30 has its own timer.
    //NOTE: The timer seems to be inaccurate, though, possibly depending on voltage. Should it be offset?
    Serial.println();
    Serial.print(F("Setting SCD30 timestep to "));
    Serial.print(config::measurement_timestep);
    Serial.println(" s.");
    scd30.setMeasurementInterval(config::measurement_timestep); // [s]

    Serial.print(F("Setting temperature offset to -"));
    Serial.print(abs(config::temperature_offset));
    Serial.println(" K.");
    scd30.setTemperatureOffset(abs(config::temperature_offset)); // setTemperatureOffset only accepts positive numbers, but shifts the temperature down.
    delay(100);

    Serial.print(F("Temperature offset is : -"));
    Serial.print(scd30.getTemperatureOffset());
    Serial.println(" K");

    Serial.print(F("Auto-calibration is "));
    Serial.println(config::auto_calibrate_sensor ? "ON." : "OFF.");

    sensor_console::defineIntCommand("co2", setCO2forDebugging, F(" 1500 (Sets co2 level, for debugging purposes)"));
    sensor_console::defineIntCommand("timer", setTimer, F(" 30 (Sets measurement interval, in s)"));
    sensor_console::defineCommand("calibrate", startCalibrationProcess, F(" (Starts calibration process)"));
    sensor_console::defineIntCommand("calibrate", calibrateSensorToSpecificPPM,
        F(" 600 (Starts calibration process, to given ppm)"));
    sensor_console::defineIntCommand("calibrate!", calibrateSensorRightNow,
        F(" 600 (Calibrates right now, to given ppm)"));
    sensor_console::defineIntCommand("auto_calibrate", setAutoCalibration,
        F(" 0/1 (Disables/enables autocalibration)"));
  }

  //NOTE: should timer deviation be used to adjust measurement_timestep?
  void checkTimerDeviation() {
    static int32_t previous_measurement_at = 0;
    int32_t now = millis();
    Serial.print(F("Measurement time offset : "));
    Serial.print(now - previous_measurement_at - config::measurement_timestep * 1000);
    Serial.println(" ms.");
    previous_measurement_at = now;
  }

  bool countStableMeasurements() {
    // Returns true, if a sufficient number of stable measurements has been observed.
    static int16_t previous_co2 = 0;
    if (co2 > (previous_co2 - config::max_deviation_during_calibration)
        && co2 < (previous_co2 + config::max_deviation_during_calibration)) {
      stable_measurements++;
      Serial.print(F("Number of stable measurements : "));
      Serial.println(stable_measurements);
      switchState(PREPARE_CALIBRATION_STABLE);
    } else {
      stable_measurements = 0;
      switchState(PREPARE_CALIBRATION_UNSTABLE);
    }
    previous_co2 = co2;
    return (stable_measurements == config::enough_stable_measurements);
  }

  void startCalibrationProcess() {
    /** From the sensor documentation:
     * For best results, the sensor has to be run in a stable environment in continuous mode at
     * a measurement rate of 2s for at least two minutes before applying the FRC command and sending the reference value.
     */
    Serial.println(F("Setting SCD30 timestep to 2s, prior to calibration."));
    scd30.setMeasurementInterval(2); // [s] The change will only take effect after next measurement.
    Serial.println(F("Waiting until the measurements are stable for at least 2 minutes."));
    Serial.println(F("It could take a very long time."));
    switchState(PREPARE_CALIBRATION_UNSTABLE);
  }

  void calibrateAndRestart() {
    switchState(CALIBRATION);
    Serial.print(F("Calibrating SCD30 now..."));
    scd30.setAltitudeCompensation(config::altitude_above_sea_level);
    scd30.setForcedRecalibrationFactor(config::co2_calibration_level);
    Serial.println(F(" Done!"));
    Serial.println(F("Sensor calibrated."));
    ESP.restart(); // softer than ESP.reset
  }

  void logToSerial() {
    Serial.print(timestamp);
    Serial.print(F(" - co2(ppm): "));
    Serial.print(co2);
    Serial.print(F(" temp(C): "));
    Serial.print(temperature, 1);
    Serial.print(F(" humidity(%): "));
    Serial.println(humidity, 1);
  }

  void switchState(state new_state) {
    if (new_state == current_state) {
      return;
    }
    if (config::debug_sensor_states) {
      Serial.print(F("Changing sensor state: "));
      Serial.print(state_names[current_state]);
      Serial.print(" -> ");
      Serial.println(state_names[new_state]);
    }
    current_state = new_state;
  }

  void displayCO2OnLedRing() {
    /**
     * Display data, even if it's "old" (with breathing).
     * A short delay is required in order to let background tasks run on the ESP8266.
     * see https://github.com/esp8266/Arduino/issues/3241#issuecomment-301290392
     */
    if (co2 < 2000) {
      led_effects::displayCO2color(co2);
      delay(100);
    } else {
      // >= 2000: entire ring blinks red
      led_effects::redAlert();
    }
  }

  void showState() {
    switch (current_state) {
    case BOOTUP:
      led_effects::showWaitingLED(color::blue);
      break;
    case READY:
      displayCO2OnLedRing();
      break;
    case NEEDS_CALIBRATION:
      led_effects::showWaitingLED(color::magenta);
      break;
    case PREPARE_CALIBRATION_UNSTABLE:
      led_effects::showWaitingLED(color::red);
      break;
    case PREPARE_CALIBRATION_STABLE:
      led_effects::showWaitingLED(color::green);
      break;
    case CALIBRATION: // Nothing to do, will restart soon.
      break;
    default:
      Serial.println(F("Encountered unknown sensor state")); // This should not happen.
    }
  }

  /** Gets fresh data if available, checks calibration status, displays CO2 levels.
   * Returns true if fresh data is available, for further processing (e.g. MQTT, CSV or LoRa)
   */
  bool processData() {
    bool freshData = scd30.dataAvailable();

    if (freshData) {
      // checkTimerDeviation();
      ntp::getLocalTime(timestamp);
      co2 = scd30.getCO2();
      temperature = scd30.getTemperature();
      humidity = scd30.getHumidity();

      if (co2 == 0) {
        // NOTE: Data is available, but it's sometimes erroneous: the sensor outputs
        // zero ppm but non-zero temperature and non-zero humidity.
        Serial.println(F("Invalid sensor data - CO2 concentration supposedly 0 ppm"));
        switchState(BOOTUP);
      } else if ((current_state == PREPARE_CALIBRATION_UNSTABLE) || (current_state == PREPARE_CALIBRATION_STABLE)) {
        // Check for pre-calibration states first, because we do not want to
        // leave them before calibration is done.
        bool ready_for_calibration = countStableMeasurements();
        if (ready_for_calibration) {
          calibrateAndRestart();
        }
      } else if (co2 < 250) {
        // Sensor should be calibrated.
        switchState(NEEDS_CALIBRATION);
      } else {
        switchState(READY);
      }

      // Log every time fresh data is available.
      logToSerial();
    }

    showState();

    return freshData;
  }

  /*****************************************************************
   * Callbacks for sensor commands                                 *
   *****************************************************************/
  void setCO2forDebugging(int32_t fakeCo2) {
    Serial.print(F("DEBUG. Setting CO2 to "));
    co2 = fakeCo2;
    Serial.println(co2);
  }

  void setAutoCalibration(int32_t autoCalibration) {
    config::auto_calibrate_sensor = autoCalibration;
    scd30.setAutoSelfCalibration(autoCalibration);
    Serial.print(F("Setting auto-calibration to : "));
    Serial.println(autoCalibration ? F("On.") : F("Off."));
  }

  void setTimer(int32_t timestep) {
    if (timestep >= 2 && timestep <= 1800) {
      Serial.print(F("Setting Measurement Interval to : "));
      Serial.print(timestep);
      Serial.println("s.");
      sensor::scd30.setMeasurementInterval(timestep);
      config::measurement_timestep = timestep;
      led_effects::showKITTWheel(color::green, 1);
    }
  }

  void calibrateSensorToSpecificPPM(int32_t calibrationLevel) {
    if (calibrationLevel >= 400 && calibrationLevel <= 2000) {
      Serial.print(F("Force calibration, at "));
      config::co2_calibration_level = calibrationLevel;
      Serial.print(config::co2_calibration_level);
      Serial.println(" ppm.");
      sensor::startCalibrationProcess();
    }
  }

  void calibrateSensorRightNow(int32_t calibrationLevel) {
    stable_measurements = config::enough_stable_measurements;
    calibrateSensorToSpecificPPM(calibrationLevel);
  }
}