#include <LiquidCrystal_I2C.h>
#include "RTClib.h"
#include <ESP32Servo.h>
#include "ThingsBoard.h"
#include <WiFi.h>

#define CURRENT_FIRMWARE_TITLE "TEST"
#define CURRENT_FIRMWARE_VERSION "1.0.0"

#define WIFI_AP_NAME              "Wokwi-GUEST"
#define WIFI_PASSWORD             ""
#define TOKEN "RIIAAofKntXnVfzyGwRB" //"Silahkan isi Token Listrik"
#define THINGSBOARD_SERVER "demo.thingsboard.io"
LiquidCrystal_I2C lcd(0x27, 20, 4);

RTC_DS1307 rtc;

#define NTC 34
#define Pump 14
#define heater 32
#define DOx 35
#define PH 33
#define ECHO 27
#define TRIG 12
#define Aerator 25
#define drain 26
#define feeder 13
#define feedDelay 3000//60000

const float BETA = 3950; // should match the Beta Coefficient of the thermistor

int status = WL_IDLE_STATUS;
WiFiClient client;
ThingsBoard tb(client);

Servo drainServo;
Servo feederServo;

int feedHr = 10, feedInterv = 1, lastFeed;
boolean feed;
int pompSpeed = 0, maxHeater = 255, minHeater = 0, valHeater;
int aerSpeed = 0, maxAer = 255, minAer = 0, valAer;

//definisi untuk Sensor o2 & PH
const float VRefer = 3.3; //Tegangan referensi untuk sensor O2

//definisi untuk Sensor PH

#define Offset 41.02740741      //deviation compensate
#define samplingInterval 20
#define printInterval 800
#define ArrayLenth  40    //times of collection
#define uart  Serial
int pHArray[ArrayLenth];   //Store the average value of the sensor feedback
int pHArrayIndex = 0;

float error;
float integralError;
float derivatifError;
float lastError = 0;
float Kp, Ki, Kd;
float KpDO, KiDO, KdDO;
float tempSet, doxSet;

float readDistanceCM() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  int duration = pulseIn(ECHO, HIGH);
  return duration * 0.034 / 2;
}

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

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

  drainServo.attach(drain);
  feederServo.attach(feeder);

  if (! rtc.begin()) {
    Serial.println("Couldn't find RTC");
    while (1);
  }
  rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));

  WiFi.disconnect();
  WiFi.begin("Wokwi-GUEST", "");
  while ((!(WiFi.status() == WL_CONNECTED))) {
    delay(300);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());

  analogReadResolution(10);
  pinMode(NTC,INPUT);
  pinMode(DOx,INPUT);
  pinMode(PH,INPUT);
  pinMode(Pump, OUTPUT);
  pinMode(heater, OUTPUT);
  pinMode(Aerator, OUTPUT);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  lcd.setCursor(6,0);
  lcd.print("PROJECT");
  lcd.setCursor(5,1);
  lcd.print("BUDIDAYA");
  lcd.setCursor(4,2);
  lcd.print("UDANG VANAME");
  lcd.setCursor(3,3);
  lcd.print("GROUP02 IOT3-9");
  
  tempSet = 28;
  doxSet = 3.5;
  Kp = 1;
  Ki = 3;
  Kd = -0.2;
  KpDO = 1;
  KiDO = 3;
  KdDO = 0.2;

  lastFeed = feedHr;
  drainServo.write(0);
  feederServo.write(0);

  delay(5000);
  
  lcd.clear();
  // resolusi jadi 10 bit untuk sensor O2,PH dan NTC
   analogReadResolution(10);
}

void loop() {

  delay(1000);
  if(WiFi.status() != WL_CONNECTED){
    reconnect();
  }

  if(!tb.connected()) {
      Serial.println("Connecting to: ");
      Serial.print(THINGSBOARD_SERVER);
      Serial.print(" with token ");
      Serial.println(TOKEN);
      if(!tb.connect(THINGSBOARD_SERVER, TOKEN)){
        Serial.println("Failed to connect");
        return;
      }
  }

  //sensor suhu NTC
  int analogNTC = analogRead(NTC);
  float celsius = 1 / (log(1 / (1023. / analogNTC - 1)) / BETA + 1.0 / 298.15) - 273.15;
  //pengaturan heater menggunakan kontrol PID
  float tempRespon = calcPID(tempSet, celsius, Kp, Ki, Kd);
  int valHeater = pompSpeed + tempRespon;
  if(valHeater >= maxHeater) valHeater = maxHeater;
  else if(valHeater <= minHeater) valHeater = minHeater;
  analogWrite(heater, valHeater);
  tb.sendTelemetryFloat("heater", valHeater);
  Serial.print("Temperature: ");
  Serial.print(celsius);
  Serial.println(" °C");
  tb.sendTelemetryFloat("Temperature", celsius );
  lcd.setCursor(0,0);
  lcd.print("Temp: " + String(celsius) + " " + char(223) + "C");
 
  //O2 Dissolve value controlling
  float analogDOx = readConcentration();
  float doxRespon = calcPID(doxSet, analogDOx, KpDO, KiDO, KdDO);
  int valAer = aerSpeed + doxRespon;
  if(valAer >= maxAer) valAer = maxAer;
  else if(valHeater <= minAer) valAer = minAer;
  analogWrite(Aerator, valAer);
  tb.sendTelemetryFloat("Aerator", valAer);
  Serial.print("DOx sens: ");
  Serial.println(analogDOx);
  tb.sendTelemetryFloat("DOx", analogDOx );
  lcd.setCursor(0,1);
  lcd.print("O2: " + String(analogDOx));

  //PH controlling
  float analogPH = readPH();
  if (analogPH < 7){
    analogWrite(Pump, 255); //add some acid?
  } else if (analogPH > 8){
    analogWrite(Pump, 255);
  } else {
    analogWrite(Pump, 20);
  }
  Serial.print("ADC PH: ");
  Serial.println(analogPH);
  tb.sendTelemetryFloat("PH", analogPH );
  lcd.setCursor(0,2);
  lcd.print("PH: " + String(analogPH));

  //Water level controlling
  float distance = readDistanceCM();
  Serial.print("Distance: ");
  Serial.println(distance);
  tb.sendTelemetryFloat("Dist", distance);
  lcd.setCursor(0,3);
  lcd.print("Dist: " + String(distance) + " cm");

  //drain control
  if(distance >= 200){
    drainServo.write(90);
    tb.sendTelemetryString("Drain", "Open");
  }else{
    drainServo.write(0);
    tb.sendTelemetryString("Drain", "Close");
  }

  //RTC and Feeder Controlling
  DateTime time = rtc.now();
  int hour = int(time.hour());
  int minute = int(time.minute());
  int second = int(time.second());
  Serial.println(String(hour) + ":" + String(minute) + ":" + String(second));
  if ((feed == 1) && (minute > lastFeed)) {
    feed = 0;
  }
  if((minute == feedHr  || minute == (lastFeed + feedInterv)) && feed == 0){
    lastFeed = int(time.minute());
    feederServo.write(90);
    tb.sendTelemetryString("Feeder", "Open");
    delay(feedDelay);
    feederServo.write(0);
    tb.sendTelemetryString("Feeder", "Close");
    feed = 1;
  }

   tb.loop(); 
}

void InitWiFi()
{
  Serial.println("Connecting to AP ...");
  // attempt to connect to WiFi network

  WiFi.begin(WIFI_AP_NAME, WIFI_PASSWORD);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected to AP");
}

void reconnect() {
  // Loop until we're reconnected
  status = WiFi.status();
  if ( status != WL_CONNECTED) {
    WiFi.begin(WIFI_AP_NAME, WIFI_PASSWORD);
    while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
    }
    Serial.println("Connected to AP");
  }
}
//https://wiki.seeedstudio.com/Grove-Gas_Sensor-O2/
float readO2Vout()
{
    long sum = 0;
    for(int i=0; i<32; i++)
    {
        sum += analogRead(DOx);
    }
    sum >>= 5;
    float MeasuredVout = sum * (VRefer / 1024);
    return MeasuredVout;
}


float readConcentration()
{
    // Vout samples are with reference to 3.3V
    float MeasuredVout = readO2Vout();
    float vDOx = MeasuredVout * 20 / 3.3; //find Dissolve Oxygen value
    return vDOx;
}

//https://wiki.seeedstudio.com/Grove-PH-Sensor-kit/

float readPH()
{
  static unsigned long samplingTime = millis();
  static unsigned long printTime = millis();
  static float pHValue, voltage;
  if (millis() - samplingTime > samplingInterval)
  {
    pHArray[pHArrayIndex++] = analogRead(PH);
    if (pHArrayIndex == ArrayLenth)pHArrayIndex = 0;
    //voltage = avergearray(pHArray, ArrayLenth) * 5.0 / 1024;
    voltage = analogRead(PH) * 3.3 / 1024;
    pHValue = -19.18518519 * voltage + Offset;
    samplingTime = millis();
  }
  return pHValue;

  //if (millis() - printTime > printInterval)  //Every 800 milliseconds, print a numerical, convert the state of the LED indicator
  //{
  //  uart.print("Voltage:");
   //uart.print(voltage, 2);
    //
    //uart.print("    pH value: ");
    //uart.println(pHValue, 2);
   // digitalWrite(LED, digitalRead(LED) ^ 1);
   // printTime = millis();
 // }
}

double avergearray(int* arr, int number) {
  int i;
  int max, min;
  double avg;
  long amount = 0;
  if (number <= 0) {
    Serial.println("Error number for the array to avraging!/n");
    return 0;
  }
  if (number < 5) { //less than 5, calculated directly statistics
    for (i = 0; i < number; i++) {
      amount += arr[i];
    }
    avg = amount / number;
    return avg;
  } else {
    if (arr[0] < arr[1]) {
      min = arr[0]; max = arr[1];
    }
    else {
      min = arr[1]; max = arr[0];
    }
    for (i = 2; i < number; i++) {
      if (arr[i] < min) {
        amount += min;      //arr<min
        min = arr[i];
      } else {
        if (arr[i] > max) {
          amount += max;  //arr>max
          max = arr[i];
        } else {
          amount += arr[i]; //min<=arr<=max
        }
      }//if
    }//for
    avg = (double)amount / (number - 2);
  }//if
  return avg;
}

float calcPID(float input, float sensor, int P, int I, int D)
{
  error = input - sensor;
  integralError += error;
  derivatifError = error - lastError;
  lastError = error;
   
  return (P * error) + (I * integralError) + (D * derivatifError);
}