#include <Wire.h>
#include "esp8266_mqtt_setup.h"
#define responseLength 32

const int wireDelayTime = 1000;
char response[responseLength];

const char* topicWaterTemp = "aquaponics/water/temperature";
const char* topicPH = "aquaponics/water/ph";
const char* topicEC = "aquaponics/water/ec";
const char* topicSalinity = "aquaponics/water/salinity";
const char* topicDO = "aquaponics/water/do";
const char* topicDOSaturation = "aquaponics/water/do_saturation";

const char* clientName = "WaterSensorClient";

const int waterTempSensorAddress = 102;
const int phSensorAddress = 99;
const int ecSensorAddress = 100;
const int doSensorAddress = 97;

bool takingWaterTempMeasurement = false;
int waterTempMeasurementDelay;
unsigned long int waterTempMeasurementLastMillis;

bool takingPHMeasurement = false;
int phMeasurementDelay;
unsigned long int phMeasurementLastMillis;

bool takingECMeasurement = false;
int ecMeasurementDelay;
unsigned long int ecMeasurementLastMillis;

bool takingDOMeasurement = false;
int doMeasurementDelay;
unsigned long int doMeasurementLastMillis;

const int MEASUREMENT_PERIOD = 5000;
unsigned long lastMeasurement = 0;


void sendCommand(const int address, const char* command) {
  Serial.print("Sending command ");
  Serial.print(command);
  Serial.print(" to ");
  Serial.println(address);
  Wire.beginTransmission(address);
  Wire.write(command);
  Wire.endTransmission();
}

void requestResponse(const int address) {
  Wire.requestFrom(address, responseLength, 1);
  int code = Wire.read();
  Serial.print(code);
  Serial.print(" ");

  switch (code) {
    case 1:
      Serial.println("Success");
      break;

    case 2:
      Serial.println("Failed");
      break;

    case 254:
      Serial.println("Pending");
      break;

    case 255:
      Serial.println("No Data");
      break;

    default:
      Serial.println("???");
      break;
  }
  
  int i = 0;
  while (Wire.available()) {
    char inchar = Wire.read();
    response[i] = inchar;
    i++;
    if (inchar == 0) {
      break;
    }
  }

  for (int j = i; j < responseLength; j++) {
    response[j] = ' ';
  }
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  delay(1000);

  sendCommand(waterTempSensorAddress, "C,0");
  delay(wireDelayTime);
  requestResponse(waterTempSensorAddress);

  sendCommand(phSensorAddress, "C,0");
  delay(wireDelayTime);
  requestResponse(phSensorAddress);

  sendCommand(ecSensorAddress, "C,0");
  delay(wireDelayTime);
  requestResponse(ecSensorAddress);

  sendCommand(doSensorAddress, "C,0");
  delay(wireDelayTime);
  requestResponse(doSensorAddress);

  sendCommand(ecSensorAddress, "O,SG,0");
  delay(wireDelayTime);
  requestResponse(ecSensorAddress);

  sendCommand(ecSensorAddress, "O,?");
  delay(wireDelayTime);
  requestResponse(ecSensorAddress);
  Serial.println(response);

  sendCommand(doSensorAddress, "O,%,1");
  delay(wireDelayTime);
  requestResponse(doSensorAddress);
  Serial.println(response);

  setup_wifi();
  setup_mqtt();
}

void loop() {
  if (!mqttClient.connected()) {
    reconnect(clientName);
  }
  mqttClient.loop();

  unsigned long currentMillis = millis();
  if (currentMillis - lastMeasurement >= MEASUREMENT_PERIOD) {
    lastMeasurement = currentMillis;
    RestartWaterTempMeasurement();
    RestartPHMeasurement();
    RestartECMeasurement();
    RestartDOMeasurement();
  }

  if (takingWaterTempMeasurement) {
    TakeWaterTempMeasurement();
  }

  if (takingPHMeasurement) {
    TakePHMeasurement();
  }

  if (takingECMeasurement) {
    TakeECMeasurement();
  }

  if (takingDOMeasurement) {
    TakeDOMeasurement();
  }
}

void RestartWaterTempMeasurement() {
  waterTempMeasurementDelay = 0;
  waterTempMeasurementLastMillis = millis();

  sendCommand(waterTempSensorAddress, "R");

  takingWaterTempMeasurement = true;
}

void TakeWaterTempMeasurement() {
  if (waterTempMeasurementDelay < wireDelayTime) {
    unsigned long int currentMillis = millis();
    waterTempMeasurementDelay += currentMillis - waterTempMeasurementLastMillis;
    waterTempMeasurementLastMillis = currentMillis;
    return;
  }
  
  requestResponse(waterTempSensorAddress);

  Serial.print("Water temperature I2C output: ");
  Serial.println(response);
  if (isdigit(response[0])) {
    float temperature = atof(response);
    Serial.print("Water temperature: ");
    Serial.print(temperature);
    Serial.println(" °C");

    mqttClient.publish(topicWaterTemp, String(temperature).c_str());
  }
  Serial.println();

  takingWaterTempMeasurement = false;
}

void RestartPHMeasurement() {
  phMeasurementDelay = 0;
  phMeasurementLastMillis = millis();

  sendCommand(phSensorAddress, "R");

  takingPHMeasurement = true;
}

void TakePHMeasurement() {
  if (phMeasurementDelay < wireDelayTime) {
    unsigned long int currentMillis = millis();
    phMeasurementDelay += currentMillis - phMeasurementLastMillis;
    phMeasurementLastMillis = currentMillis;
    return;
  }

  requestResponse(phSensorAddress);

  Serial.print("PH I2C output: ");
  Serial.println(response);
  if (isdigit(response[0])) {
    float ph = atof(response);
    Serial.print("PH: ");
    Serial.println(ph);

    mqttClient.publish(topicPH, String(ph).c_str());
  }
  Serial.println();

  takingPHMeasurement = false;
}

void RestartECMeasurement() {
  ecMeasurementDelay = 0;
  ecMeasurementLastMillis = millis();

  sendCommand(ecSensorAddress, "R");

  takingECMeasurement = true;
}

void TakeECMeasurement() {
  if (ecMeasurementDelay < wireDelayTime) {
    unsigned long int currentMillis = millis();
    ecMeasurementDelay += currentMillis - ecMeasurementLastMillis;
    ecMeasurementLastMillis = currentMillis;
    return;
  }

  requestResponse(ecSensorAddress);

  Serial.print("EC I2C output: ");
  Serial.println(response);
  if (isdigit(response[0])) {
    char* ecString = strtok(response, ",");
    char* salinityString = strtok(NULL, ",");
    
    float ec = atof(ecString);
    float salinity = atof(salinityString);

    Serial.print("EC: ");
    Serial.print(ec);
    Serial.println(" μS/cm");

    Serial.print("Salinity: ");
    Serial.print(salinity);
    Serial.println(" PSU");


    mqttClient.publish(topicEC, String(ec).c_str());
    mqttClient.publish(topicSalinity, String(salinity).c_str());
  }
  Serial.println();

  takingECMeasurement = false;
}

void RestartDOMeasurement() {
  doMeasurementDelay = 0;
  doMeasurementLastMillis = millis();

  sendCommand(doSensorAddress, "R");

  takingDOMeasurement = true;
}

void TakeDOMeasurement() {
  if (doMeasurementDelay < wireDelayTime) {
    unsigned long int currentMillis = millis();
    doMeasurementDelay += currentMillis - doMeasurementLastMillis;
    doMeasurementLastMillis = currentMillis;
    return;
  }

  requestResponse(doSensorAddress);

  Serial.print("DO I2C output: ");
  Serial.println(response);
  if (isdigit(response[0])) {
    char* doString = strtok(response, ",");
    char* saturationString = strtok(NULL, ",");
    
    float do_ = atof(doString);
    float saturation = atof(saturationString);

    Serial.print("DO: ");
    Serial.print(do_);
    Serial.println(" mg/l");

    Serial.print("Saturation: ");
    Serial.print(saturation);
    Serial.println(" %");


    mqttClient.publish(topicDO, String(do_).c_str());
    mqttClient.publish(topicDOSaturation, String(saturation).c_str());
  }
  Serial.println();

  takingDOMeasurement = false;
}