#include <pas-co2-ino.hpp>
#include <ArduinoIoTCloud.h>
#include "Adafruit_HDC1000.h"

#include "ArduinoGraphics.h"
#include "Arduino_LED_Matrix.h"

#include <FastLED.h>

#define WIFI_SSID "xxxxx"
#define WIFI_PASS "yyyyyyyyyyyyyyy"

#define ARDUINO_CLOUD_DEVICE_ID "aaaaaaaa-aaaa-aaaa-aaaa-aaaaaaaaaaaa"
#define ARDUINO_CLOUD_DEVICE_SECRET_KEY "XXXXXXXXXXXXXXXXXXXXXXXXX"

PASCO2Ino co2_sensor(&Wire, 2);
Adafruit_HDC1000 temp_humi_sensor = Adafruit_HDC1000();

WiFiConnectionHandler ArduinoCloudConnectionHandler(WIFI_SSID, WIFI_PASS);

int co2;
bool is_co2_ok;

float pressure;
bool is_pressure_ok;

float temperature;
bool is_temperature_ok;

float humidity;
bool is_humidity_ok;

ArduinoLEDMatrix matrix;

// array of LEDs color. It is transition Green-Orange-Red-Violet
CRGB leds_template[15] = {0x008800, 0x00ff00, 0x59e500, 0x9cd200, 0xbdc800, 0xe59300, 0xff7100, 0xff5f00, 0xff4b00, 0xff2a00, 0xff2000, 0xff1200, 0xff0000, 0xd90051, 0x520099};
CRGB leds[15];

int co2_led_levels[16] = {0, 200, 400, 600, 800, 1000, 1200, 1400, 1600, 1800, 2000, 2400, 2800, 3500, 5000, INT_MAX};

int wait_to;

void setup() {
  Error_t status;

  Serial.begin(9600);

  setDebugMessageLevel(DBG_INFO);

  Serial.println("Init begin");

  ArduinoCloud.setBoardId(ARDUINO_CLOUD_DEVICE_ID);
  ArduinoCloud.setSecretDeviceKey(ARDUINO_CLOUD_DEVICE_SECRET_KEY);
  ArduinoCloud.addProperty(co2, Permission::Read).publishOnChange(0);
  ArduinoCloud.addProperty(is_co2_ok, Permission::Read).publishOnChange(1);
  ArduinoCloud.addProperty(pressure, Permission::Read).publishOnChange(0);
  ArduinoCloud.addProperty(is_pressure_ok, Permission::Read).publishOnChange(1);
  ArduinoCloud.addProperty(temperature, Permission::Read).publishOnChange(0.001);
  ArduinoCloud.addProperty(is_temperature_ok, Permission::Read).publishOnChange(1);
  ArduinoCloud.addProperty(humidity, Permission::Read).publishOnChange(0.001);
  ArduinoCloud.addProperty(is_humidity_ok, Permission::Read).publishOnChange(1);
  ArduinoCloud.printDebugInfo();
  ArduinoCloud.begin(ArduinoCloudConnectionHandler);

  Wire.begin();

  matrix.begin();

  FastLED.addLeds<NEOPIXEL, 6>(leds, 15);

  // power up LED strip test sequence
  int direction = 1;
  for (int i = 0; i < 15 && i >= 0; i += direction) {
    co2 = co2_led_levels[i];
    update_co2_ledstrip(false);
    delay(50);

    if (i == 14) {
      direction = -1;
      delay(300);
    }
  }
  leds[0] = CRGB::Black;
  FastLED.show();
  
  matrix.loadSequence(LEDMATRIX_ANIMATION_LOAD);
  matrix.begin();
  matrix.play(true);

  // Wait for sensor powerup.
  delay(6000);

  status = co2_sensor.begin();
  if (status) {
    Serial.print("PAS CO2 sensor init failed with status code: ");
    Serial.println(status);

    // It do not make sense to continue without CO2 sensor. Trigger reboot
    reboot();
  }

  status = co2_sensor.startMeasure(60);
  if (status) {
    Serial.print("PAS CO2 sensor failed when starting measurement with status code: ");
    Serial.println(status);

    // It do not make sense to continue without CO2 sensor. Trigger reboot
    reboot();
  }

  init_temperature_and_humidity();

  Serial.println("Init done");

  // Wait for first measurement
  delay(61000);
  
  matrix.loadFrame(LEDMATRIX_LIKE);
  delay(1000);

  matrix.clear();

  wait_to = millis() + 30000;
}

void loop() {
  recover_iic();

  measure_pressure();
  measure_co2();
  measure_temperature_and_humidity();

  // following code prints result on display twice
  // print results to display every 30 sec, 2x60sec = 60sec which is sample period that PAS CO2 is configured to.
  for (int i = 0; i < 2; i++) {
    // send data to cloud first. 1 second time dedicated to cloud send should be enough for most Wi-Fi networks.
    delay_with_cloud_update(1000);

    show_results();
    update_co2_ledstrip(true);

    while (millis() < wait_to) {
      ArduinoCloud.update();
    }
    wait_to += 30000;
  }
}

void measure_co2() {
  Error_t status;
  int16_t co2;

  status = co2_sensor.getCO2(co2);
  if (status == XENSIV_PASCO2_OK) {
    set_co2_value(co2);
  } else {
    if (status == XENSIV_PASCO2_ERR_COMM) {
      delay(1000);
      wait_to += 1000;

      status = co2_sensor.getCO2(co2);
      if (status == XENSIV_PASCO2_OK) {
        set_co2_value(co2);
      } else {
        set_co2_faulty(status);
        return;
      }
    } else {
      set_co2_faulty(status);
      return;
    }
  }

  int pressure_int = (int)roundf(pressure);
  co2_sensor.setPressRef(pressure_int);
}

void set_co2_value(int16_t from_sensor) {
  co2 = from_sensor;
  is_co2_ok = true;

  Serial.print("CO2: ");
  Serial.print(from_sensor);
  Serial.println(" ppm");
}

void set_co2_faulty(Error_t status) {
  is_co2_ok = false;

  Serial.print("PAS CO2 sensor reading CO2 failed with status code: ");
  Serial.println(status);
  delay(100);
}

void init_pressure() {
  // Reset sensor
  Wire.beginTransmission(0x5C);
  Wire.write(0x11);
  Wire.write(0x04);
  if (Wire.endTransmission()) {
    Serial.println("Reseting ILOS22QS failed.");
    is_pressure_ok = false;
    return;
  }

  delay(100);

  // Configure ODR=10Hz, AVG=4samples
  Wire.beginTransmission(0x5C);
  Wire.write(0x10);
  Wire.write(0x18);
  Wire.write(0x18);
  if (Wire.endTransmission()) {
    Serial.println("Configuring ILOS22QS failed.");
    is_pressure_ok = false;
    return;
  }

  is_pressure_ok = true;

  // wait for first sample
  delay(500);
}

void measure_pressure() {
  // try reinitialize if sensor failed recently
  if (!is_pressure_ok) {
    init_pressure();
  }

  // if it is still broken, do not proceed to reading.
  if (!is_pressure_ok) {
    return;
  }

  // Read status
  Wire.beginTransmission(0x5C);
  Wire.write(0x27);
  if (Wire.endTransmission(false)) {
    Serial.println("Reading ILOS22QS status failed.");
    is_pressure_ok = false;
    return;
  }
  Wire.requestFrom(0x5C, 1);
  if (!Wire.available()) {
    Serial.println("Reading ILOS22QS status failed.");
    is_pressure_ok = false;
    return;
  }
  uint8_t status = Wire.read();

  if ((status & 0x01) == 0) {
    Serial.println("ILOS22QS did not provide any new data.");
    return;
  }

  // Read pressure
  Wire.beginTransmission(0x5C);
  Wire.write(0x28);
  if (Wire.endTransmission(false)) {
    Serial.println("Reading pressure value from ILOS22QS failed.");
    is_pressure_ok = false;
    return;
  }
  Wire.requestFrom(0x5C, 3);
  // uint32_t instead of uint8_t is here for reducing need for casting later when converting 3 bytes to 24-bit value.
  uint32_t bytes[3]; 
  for (int i = 0; i < 3; i++) {
    if (!Wire.available()) {
      Serial.println("Reading pressure value from ILOS22QS failed.");
      is_pressure_ok = false;
      return;
    }
    bytes[i] = Wire.read();
  }
  uint32_t value = 
    (bytes[0] << 0) |
    (bytes[1] << 8) |
    (bytes[2] << 16);

  pressure = (float)value / 4096.0;

  Serial.print("Pressure: ");
  Serial.print(pressure);
  Serial.println(" hPa");
}

void init_temperature_and_humidity() {
  bool status = temp_humi_sensor.begin();
  is_temperature_ok = status;
  is_humidity_ok = status;
}

void measure_temperature_and_humidity() {
  // try initialize if not yet (or previous attemp was unsuccessfull)
  if (!is_temperature_ok) {
    init_temperature_and_humidity();
  }

  // if it is still broken, do not proceed to reading.
  if (!is_temperature_ok) {
    return;
  }

  float new_temperature = temp_humi_sensor.readTemperature();
  float new_humidity = temp_humi_sensor.readHumidity();

  if (new_temperature == -40.0 && new_humidity == 0.0) {
    Serial.println("HDC1080 Failed.");
    is_temperature_ok = false;
    is_humidity_ok = false;
    return;
  }

  temperature = new_temperature;
  humidity = new_humidity;

  Serial.print("Temperature: ");
  Serial.print(temperature);
  Serial.println(" °C");

  Serial.print("Humidity: ");
  Serial.print(humidity);
  Serial.println(" %");
}

void show_results() {
  char buff_co2[32] = {'\0'};
  char buff_pressure[32] = {'\0'};
  char buff_temperature[32] = {'\0'};
  char buff_humidity[32] = {'\0'};

  char buf_final[sizeof(buff_co2) + sizeof(buff_pressure) + sizeof(buff_temperature) + sizeof(buff_humidity) + 16];

  bool has_value = false;

  if (is_co2_ok) {
    snprintf(buff_co2, sizeof(buff_co2), "CO2:%d ppm", co2);
    has_value = true;
  }
  
  if (is_pressure_ok) {
    snprintf(buff_pressure, sizeof(buff_pressure), "Pressure:%.2f hPa", pressure);
    has_value = true;
  }
  
  if (is_temperature_ok) {
    snprintf(buff_temperature, sizeof(buff_temperature), "Temperature:%.1fC", temperature);
    snprintf(buff_humidity, sizeof(buff_humidity), "Humidity:%.0f%%", humidity);
    has_value = true;
  }

  matrix.beginDraw();
  matrix.stroke(0xFFFFFFFF);
  matrix.textScrollSpeed(50);
  matrix.textFont(Font_5x7);
  matrix.beginText(0, 1, 0xFFFFFF);
  if (!has_value) {
    const char text[] = "    Error!    ";
    matrix.println(text);
  } else {
    snprintf(buf_final, sizeof(buf_final), "    %s %s %s %s    ", buff_co2, buff_pressure, buff_temperature, buff_humidity);
    matrix.println(buf_final);
  }
  
  matrix.endText(SCROLL_LEFT);
  matrix.endDraw();
}

void update_co2_ledstrip(bool log) {
  int on = 0;
  int off = 0;
  for (int i = 0; i < 15; i++) {
    if (co2 < co2_led_levels[i]) {
      leds[i] = CRGB::Black;
      off++;
    } else {
      leds[i] = leds_template[i];
      on++;
    }
  }
  FastLED.show();

  if (log) {
    Serial.print("LED strip update done. ");
    Serial.print(on);
    Serial.print(" LEDs are ON. ");
    Serial.print(off);
    Serial.println(" LEDs are OFF.");
  }
}

void delay_with_cloud_update(int ms) {
  int start = millis() + ms;
  while (millis() < start) {
    ArduinoCloud.update();
  }
}

void recover_iic() {
  Wire.end();
  Wire.begin();
  Wire.setClock(100000);

  // Wire is IIC1
  // Wire1 is IIC0

  // Clock out 9 cycles for releasing stuck slave
  for (int i = 0; i < 9; i++) {
    R_IIC1->ICCR1_b.CLO = 1;
    int timeout = 10;
    while (R_IIC1->ICCR1_b.CLO == 1 && timeout--) {
      delayMicroseconds(10);
    }
  }
  
  // Reset master controller
  R_IIC1->ICCR1_b.IICRST = 1;
  
  Wire.end();
  Wire.begin();
  Wire.setClock(100000);
}

void reboot() {
  while (1) {
    delay(1000);
    NVIC_SystemReset();
  }
}