#include <LiquidCrystal_I2C.h>
#include "RTClib.h"
#include <ESP32Servo.h>
#include "ThingsBoard.h"
#include <WiFi.h>
// PID Temperature Heater
float kesalahan;
float integralKesalahan;
float derivatifKesalahan;
float kesalahanLalu = 0;
float Kp, Ki, Kd;
float setPointTemp;
// PID Temperature Heater EOL 
//konstanta Temperature
#define Ksensor  1 //100/1023 //faktor koreksi sensor panas

// PID DO Aerator
float kesalahanDO;
float integralKesalahanDO;
float derivatifKesalahanDO;
float kesalahanLaluDO = 0;
float KpDO, KiDO, KdDO;
float setPointDO;

//konstanta DO
#define KsensorDO   9/1023 //faktor koreksi scaling sensor DO saat full scale potensio


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

#define SSID "Wokwi-GUEST"
#define PASS ""

#define TOKEN "QMwZYoFRlQgRXbOK5u7T" //"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
int heater = 32;
#define DOx 35
#define PH 33
#define ECHO 27
#define TRIG 12
#define Aerator 25
#define drain 26
#define feeder 13

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;

//code percobaan Port PWM
const uint8_t PROGMEM gamma8[] = {
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,
  1,  1,  1,  1,  1,  1,  1,  1,  1,  2,  2,  2,  2,  2,  2,  2,
  2,  3,  3,  3,  3,  3,  3,  3,  4,  4,  4,  4,  4,  5,  5,  5,
  5,  6,  6,  6,  6,  7,  7,  7,  7,  8,  8,  8,  9,  9,  9, 10,
  10, 10, 11, 11, 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16,
  17, 17, 18, 18, 19, 19, 20, 20, 21, 21, 22, 22, 23, 24, 24, 25,
  25, 26, 27, 27, 28, 29, 29, 30, 31, 32, 32, 33, 34, 35, 35, 36,
  37, 38, 39, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 50,
  51, 52, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 66, 67, 68,
  69, 70, 72, 73, 74, 75, 77, 78, 79, 81, 82, 83, 85, 86, 87, 89,
  90, 92, 93, 95, 96, 98, 99, 101, 102, 104, 105, 107, 109, 110, 112, 114,
  115, 117, 119, 120, 122, 124, 126, 127, 129, 131, 133, 135, 137, 138, 140, 142,
  144, 146, 148, 150, 152, 154, 156, 158, 160, 162, 164, 167, 169, 171, 173, 175,
  177, 180, 182, 184, 186, 189, 191, 193, 196, 198, 200, 203, 205, 208, 210, 213,
  215, 218, 220, 223, 225, 228, 231, 233, 236, 239, 241, 244, 247, 249, 252, 255
};

void reconnect(){
  status = WiFi.status();
  if(status!=WL_CONNECTED){
    WiFi.begin(SSID, PASS);
    while(WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.println(".");
    }
    Serial.println("Connected to AP");
  }
}

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() {
  //PID Temperature Heater
  setPointTemp = 28;
  Kp = 1;
  Ki = 3;
  Kd = -0.2;
// PID Temperature Heater EOL


//PID DO Aerator
  setPointDO = 8;
  KpDO = 1;
  KiDO = 3;
  KdDO = -0.2;
// PID DO Aerator EOL


  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(__DATE__, __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");
  delay(5000);
  lcd.clear();

}

void loop() {
  // Baca NTC Sensor temp
  int analogNTC = analogRead(NTC);
  float celsius = 1 / (log(1 / (1023. / analogNTC - 1)) / BETA + 1.0 / 298.15) - 273.15;
  Serial.print("Temperature: ");
  Serial.print(celsius);
  Serial.println(" °C");
  
  // PID Temperature Heater
  float sensor = 1.0 * celsius * Ksensor;
  int respon = hitungPID(setPointTemp, sensor);
  analogWrite(heater, respon);
  Serial.print("Setpoint ");
  Serial.println(setPointTemp);
  Serial.print("Feedback PID ");
  Serial.println(sensor);
  Serial.print("PID Out Heater ");
  if(respon > 100.0) {respon = 100.0;}; // Scaling ala-ala :D
  if(respon < 0.1) {respon = 0.0;}; // Scaling ala-ala :D
  Serial.println(respon);
  delay(1000);
  Serial.println();
  Serial.println();
  Serial.println();
  lcd.setCursor(0,0); // temperature sensor
  lcd.print("Tem: ");
  lcd.setCursor(4,0);
  lcd.print(int(sensor));
  lcd.setCursor(7,0); // set point Temp
  lcd.print("SP: ");
  lcd.setCursor(10,0);
  lcd.print(int(setPointTemp));
  lcd.setCursor(13,0); // PID Out Heater
  lcd.print("Htr: ");
  lcd.setCursor(17,0);
  lcd.print(int(respon));
  // PID Temperature Heater EOL

  int analogDOx = analogRead(DOx);
  Serial.print("ADC DOx: ");
  Serial.println(analogDOx);  
  // PID DO Aerator
  float sensorDO = 1 * analogDOx * KsensorDO;
  int responDO = hitungPID_DO(setPointDO, sensorDO);
  Serial.print("PID DOx & Aerator Speed");
  analogWrite(Aerator, responDO);
  Serial.print("Setpoint DO ");
  Serial.println(setPointDO);
  Serial.print("Feedback PID DOx ");
  Serial.println(sensorDO);
  Serial.print("PID Out Speed Aerator ");
  if(responDO > 100.0) {responDO = 100.0;}; // Scaling ala-ala :D
  if(responDO < 50.0) {responDO = 50.0;}; // Batas minimal speed aerator
  Serial.println(responDO);
  delay(1000);
  lcd.setCursor(0,1); // DO sensor
  lcd.print("DO:");
  lcd.setCursor(3,1);
  lcd.print(int(sensorDO));
  lcd.setCursor(5,1); // set point DO
  lcd.print("SP:");
  lcd.setCursor(8,1);
  lcd.print(int(setPointDO));
  lcd.setCursor(10,1); // PID Out Aerator
  lcd.print("Aeratr: ");
  lcd.setCursor(17,1);
  lcd.print(int(responDO));
  
  Serial.println();
  Serial.println();
  Serial.println();
  // PID Temperature Heater EOL

  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;
      }
  }





  int analogPH = analogRead(PH);
  Serial.print("ADC PH: ");
  Serial.println(analogPH);

  float distance = readDistanceCM();
  Serial.print("Jarak: ");
  Serial.println(distance);

  if(distance >= 200){
    drainServo.write(90);
    feederServo.write(90);
  }else{
    drainServo.write(0);
    feederServo.write(0);
  }

  for (int i = 0; i < 255; i++) {
    analogWrite(Pump, pgm_read_byte(&gamma8[i]));
    //analogWrite(heater, pgm_read_byte(&gamma8[i]));
    //analogWrite(Aerator, pgm_read_byte(&gamma8[i]));
    delay(5);
  }
  for (int i = 255; i > 0; i--) {
    analogWrite(Pump, pgm_read_byte(&gamma8[i]));
    //analogWrite(heater, pgm_read_byte(&gamma8[i]));
    //analogWrite(Aerator, pgm_read_byte(&gamma8[i]));
    delay(5);
  }

  DateTime time = rtc.now();

  //Full Timestamp
  Serial.println(String("DateTime::TIMESTAMP_FULL:\t")+time.timestamp(DateTime::TIMESTAMP_FULL));

  //Date Only
  Serial.println(String("DateTime::TIMESTAMP_DATE:\t")+time.timestamp(DateTime::TIMESTAMP_DATE));

  //Full Timestamp
  Serial.println(String("DateTime::TIMESTAMP_TIME:\t")+time.timestamp(DateTime::TIMESTAMP_TIME));

  Serial.println("\n");

}

// PID Temperature Out
float hitungPID(float setPointTemp, float sensor)
{
  kesalahan = setPointTemp - sensor;
  integralKesalahan += kesalahan;
  derivatifKesalahan = kesalahan - kesalahanLalu;
  kesalahanLalu = kesalahan;
   
  return (Kp * kesalahan) + (Ki * integralKesalahan) + (Kd * derivatifKesalahan);
}
// PID Temperature Out EOL

// PID DO Out
float hitungPID_DO(float setPointDO, float sensorDO)
{
  kesalahanDO = setPointDO - sensorDO;
  integralKesalahanDO += kesalahanDO;
  derivatifKesalahanDO = kesalahanDO - kesalahanLaluDO;
  kesalahanLaluDO = kesalahanDO;
   
  return (KpDO * kesalahanDO) + (KiDO * integralKesalahanDO) + (KdDO * derivatifKesalahanDO);
}
// PID Temperature Out EOL