#include "co2_sensor.h"

#include "web_config.h"
#include "ntp.h"
#include "led_effects.h"
#include "sensor_console.h"

#include "src/lib/S8_UART/s8_uart.h"

namespace config {
  const uint16_t measurement_timestep_bootup = 4; // [s] Measurement timestep during acclimatization.
  const uint8_t max_deviation_during_bootup = 20; // [%]
  const int8_t max_deviation_during_calibration = 30; // [ppm]
  const int16_t timestep_during_calibration = 10; // [s] WARNING: Measurements can be unreliable for timesteps shorter than 10s.
  const int8_t stable_measurements_before_calibration = 120 / timestep_during_calibration; // [-] Stable measurements during at least 2 minutes.
  const uint16_t co2_alert_threshold = 2000; // [ppm] Display a flashing led ring, if concentration exceeds this value
  const bool debug_sensor_states = false; // If true, log state transitions over serial console
}

#if defined(ESP8266)
// ???
#endif
#if defined(ESP32)
// For ESP32 : RX on GPIO17, TX on GPIO16
#   define S8_UART_PORT  2
#endif

namespace sensor {
  HardwareSerial S8_serial(S8_UART_PORT);
  S8_UART *sensor_S8;
  S8_sensor s8;
  uint16_t co2 = 0;
  float temperature = 0;
  float humidity = 0;
  char timestamp[23];
  int16_t stable_measurements = 0;
  // I'm not sure it's possible to change S8 measurement interval (constant 4s). But we can check every check_timestep seconds.
  uint16_t check_timestep = 0;

  /**
   * Define sensor states
   * BOOTUP -> initial state, until first >0 ppm values are returned
   * 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_UNSTABLE -> forced calibration was initiated, last measurements were too far apart
   * PREPARE_CALIBRATION_STABLE -> forced calibration was initiated, last measurements were close to each others
   */
  enum state {
    BOOTUP,
    READY,
    NEEDS_CALIBRATION,
    PREPARE_CALIBRATION_UNSTABLE,
    PREPARE_CALIBRATION_STABLE
  };
  const char *state_names[] = {
      "BOOTUP",
      "READY",
      "NEEDS_CALIBRATION",
      "PREPARE_CALIBRATION_UNSTABLE",
      "PREPARE_CALIBRATION_STABLE" };

  state current_state = BOOTUP;
  void switchState(state);
  void setCO2forDebugging(int32_t fakeCo2);
  void calibrateSensorToSpecificPPM(int32_t calibrationLevel);
  void calibrateSensorRightNow(int32_t calibrationLevel);
  void setAutoCalibration(int32_t autoCalibration);
  void setTimer(int32_t timestep);

  void initialize() {
    Serial.println(F("Sensor   : Senseair S8"));
    S8_serial.begin(S8_BAUDRATE);
    sensor_S8 = new S8_UART(S8_serial);
    Serial.println();

    // Check if S8 is available
    sensor_S8->get_firmware_version(s8.firm_version);
    int len = strlen(s8.firm_version);
    if (len == 0) {
      Serial.println(F("ERROR - Senseair S8 CO2 sensor not detected. Please check wiring!"));
      led_effects::showKITTWheel(color::red, 30);
      ESP.restart();
    }

    // Show basic S8 sensor info
    Serial.print(F("S8 - Firmware : "));
    Serial.println(s8.firm_version);
    s8.sensor_id = sensor_S8->get_sensor_ID();
    Serial.print(F("S8 - ID : 0x"));
    printIntToHex(s8.sensor_id, 4);
    Serial.println();

    //TODO: Auto-calibration on/off?

    // S8 has its own timer (constant 4s)
    Serial.println();
    Serial.print(F("Setting S8 timestep to "));
    Serial.print(config::measurement_timestep_bootup);
    Serial.println(F(" s during acclimatization."));
    check_timestep = config::measurement_timestep_bootup;

    sensor_console::defineIntCommand("co2", setCO2forDebugging, F("1500 (Sets co2 level, for debugging)"));
    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)"));
  }

  bool hasSensorSettled() {
    static uint16_t last_co2 = 0;
    uint16_t delta;
    delta = abs(co2 - last_co2);
    last_co2 = co2;
    // We assume the sensor has acclimated to the environment if measurements
    // change less than a specified percentage of the current value.
    return (co2 > 0 && delta < ((uint32_t) co2 * config::max_deviation_during_bootup / 100));
  }

  bool enoughStableMeasurements() {
    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.print(stable_measurements);
      Serial.print(F(" / "));
      Serial.println(config::stable_measurements_before_calibration);
      switchState(PREPARE_CALIBRATION_STABLE);
    } else {
      stable_measurements = 0;
      switchState(PREPARE_CALIBRATION_UNSTABLE);
    }
    previous_co2 = co2;
    return (stable_measurements == config::stable_measurements_before_calibration);
  }

  void startCalibrationProcess() {
    Serial.println(F("Implement ME!"));
  }

  void calibrate() {
    Serial.println(F("Implement ME!"));
  }

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

  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(F(" -> "));
      Serial.println(state_names[new_state]);
    }
    current_state = new_state;
  }

  void switchStateForCurrentPPM() {
    if (current_state == BOOTUP) {
      if (!hasSensorSettled()) {
        return;
      }
      switchState(READY);
      Serial.println(F("Sensor acclimatization finished."));
      Serial.print(F("Setting S8 timestep to "));
      Serial.print(config::measurement_timestep);
      Serial.println(F(" s."));
      check_timestep = config::measurement_timestep; // [s]
    }

    // Check for pre-calibration states first, because we do not want to
    // leave them before calibration is done.
    if ((current_state == PREPARE_CALIBRATION_UNSTABLE) || (current_state == PREPARE_CALIBRATION_STABLE)) {
      if (enoughStableMeasurements()) {
        calibrate();
      }
    } else if (co2 < 250) {
      // Sensor should be calibrated.
      switchState(NEEDS_CALIBRATION);
    } else {
      switchState(READY);
    }
  }

  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 < config::co2_alert_threshold) {
      led_effects::displayCO2color(co2);
      delay(100);
    } else {
      // Display a flashing led ring, if concentration exceeds a specific value
      led_effects::alert(color::red);
    }
  }

  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;
    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() {
    static unsigned long last_measurement = 0;
    unsigned long now = seconds();
    bool freshData = now - last_measurement > check_timestep;
    if (freshData) {
      last_measurement = now;
      ntp::getLocalTime(timestamp);
      co2 = sensor_S8->get_co2();
      //TODO: Check if there's really no temperature info available.
      temperature = 0.0;
      humidity = 0.0;

      switchStateForCurrentPPM();

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

    showState();

    // Report data for further processing only if the data is reliable
    // (state 'READY') or manual calibration is necessary (state 'NEEDS_CALIBRATION').
    return freshData && (current_state == READY || current_state == NEEDS_CALIBRATION);
  }

  float getTemperatureOffset() {
    return 0.0;
  }

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

  void setAutoCalibration(int32_t autoCalibration) {
    Serial.println(F("TODO: Implement ME!"));
  }

  void setTimer(int32_t timestep) {
    if (timestep >= 4) {
      Serial.print(F("Setting Measurement Interval to : "));
      Serial.print(timestep);
      Serial.println(F("s."));
      check_timestep = timestep;
      config::measurement_timestep = timestep;
      led_effects::showKITTWheel(color::green, 1);
    }
  }

  void calibrateSensorToSpecificPPM(int32_t calibrationLevel) {
    Serial.println(F("TODO: Implement ME!"));
  }

  void calibrateSensorRightNow(int32_t calibrationLevel) {
    Serial.println(F("TODO: Implement ME!"));
  }
}