#include <Wire.h> 
#include <ArduinoJson.h>
#include <TEWeatherShield.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);
static TEWeatherShield weatherShield;

int command = 0;
int led = 1;
bool mode = true;
unsigned long previousMillis = 0;
const long interval = 10000;

void setup() {
  Serial.begin(9600);
  weatherShield.begin();

  lcd.init();
  lcd.backlight();
}

// Measure from HTU21D
float measure_htu21d(String data) {
  float temperature;
  float humidity;
  
  weatherShield.selectHTU21D();

  if (weatherShield.HTU21D.is_connected()) {
    weatherShield.HTU21D.set_i2c_master_mode(htu21_i2c_no_hold);       
    weatherShield.HTU21D.read_temperature_and_relative_humidity(&temperature, &humidity); 

    if (data == "temp") {
      return temperature;
    } else if (data == "humi") {
      return humidity;
    }
  }
  else {
    lcd.setCursor(2, 0);
    lcd.print("Napaka senzorja 1!");
  }
}

// Measure from MS5637
float measure_ms5637(String data) {
  float temperature;
  float pressure;
  
  weatherShield.selectMS5637();

  if (weatherShield.MS5637.is_connected()) {    
    weatherShield.MS5637.read_temperature_and_pressure(&temperature, &pressure);

    if (data == "temp") {
      return temperature;
    } else if (data == "pres") {
      return pressure;
    }
  }
  else {
    lcd.setCursor(2, 0);
    lcd.print("Napaka senzorja 2!");  }
}

// Measure from MS8607
float measure_ms8607(String data) {
  float temperature;
  float pressure;
  float humidity;
  
  weatherShield.selectMS8607();

  if (weatherShield.MS8607.is_connected()) {    
    weatherShield.MS8607.set_humidity_i2c_master_mode(ms8607_i2c_hold); 
    weatherShield.MS8607.read_temperature_pressure_humidity( &temperature, &pressure, &humidity);
    
    if (data == "temp") {
      return temperature;
    } else if (data == "humi") {
      return humidity;
    } else if (data == "pres") {
      return pressure;
    }
  } else {
    lcd.setCursor(2, 0);
    lcd.print("Napaka senzorja 3!");
  }
}

// Measure from TSYS01
float measure_tsys01(String data) {
  float temperature;
  
  weatherShield.selectTSYS01();

  if (weatherShield.TSYS01.is_connected()) {    
    weatherShield.TSYS01.read_temperature(&temperature);
    
    if (data == "temp") {
      return temperature;
    }
  } else {
    lcd.setCursor(2, 0);
    lcd.print("Napaka senzorja 4!");
  }
}

// Measure from TSD305
float measure_tsd305(String data) {
  float temperature;
  float object_temperature;
  
  weatherShield.selectTSD305();

  if (weatherShield.TSD305.is_connected()) {    
    weatherShield.TSD305.read_temperature_and_object_temperature(&temperature, &object_temperature);
    
    if (data == "temp") {
      return temperature;
    } else if (data == "obj_temp") {
      return object_temperature;
    }
  } else {
    lcd.setCursor(2, 0);
    lcd.print("Napaka senzorja 5!");
  }
}

// Print temperature and humidity to LCD
void printLCD(float temperature, float humidity, float pressure) {
    lcd.clear();

    String temp = "T: " + (String)temperature + " C";
    lcd.setCursor(2, 0);
    lcd.print(temp);

    if (mode == true) {
      String hum = "H: " + (String)humidity + " %RH";
      lcd.setCursor(2, 1);
      lcd.print(hum);
      mode = false;
    } else {
      String pres = "P: " + (String)pressure + " hPa";
      lcd.setCursor(2, 1);
      lcd.print(pres);
      mode = true;
    }
}

float avg(String data) {
  if (data == "temp") {
    float avgTemp = (measure_htu21d("temp") + measure_ms5637("temp") + measure_ms8607("temp") + measure_tsys01("temp") + measure_tsd305("temp")) / 5;
    return avgTemp;
  } else if (data == "humi") {
    float avgHum = (measure_htu21d("humi") + measure_ms8607("humi")) / 2;
    return avgHum;
  } else if (data == "pres") {
    float avgPres = (measure_ms8607("pres") + measure_ms5637("pres")) / 2;
    return avgPres;
  }
}

void toggleLed() {
  if (led == 0) {
    lcd.setBacklight(HIGH);
    led = 1;
  } else {
    lcd.setBacklight(LOW);
    led = 0;
  }
}

void loop() {    
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    StaticJsonDocument<100> jsonBuffer;

    float temperature = avg("temp");
    float pressure = avg("pres");
    float humidity = avg("humi");

    // Don't print to LCD if backlight is off
    if (led == 1) {
      printLCD(temperature, humidity, pressure);
    }

    // Get data and send it over serial
    jsonBuffer["temperature"] = temperature;
    jsonBuffer["humidity"] = humidity;
    jsonBuffer["pressure"] = pressure;

    serializeJson(jsonBuffer, Serial);
    Serial.println();
  }

  Serial.flush();
  command = Serial.read();
  // 84 = T
  if (command == 84) {
    toggleLed();
    //Serial.println("LED toggled");
    //Make this return some JSON state of the LED to Home Assistant
  }
}