#include <DHTesp.h>         // DHT for ESP32 library
#include <WiFi.h>           // WiFi control for ESP32
//#include <ThingsBoard.h>    // ThingsBoard SDK
#include "ThingsBoard.h"
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
Servo servo1;
Servo servo2;
Servo servo3;
#define COUNT_OF(x) ((sizeof(x)/sizeof(0[x])) / ((size_t)(!(sizeof(x) % sizeof(0[x])))))

#define WIFI_AP_NAME              "Wokwi-GUEST"
#define WIFI_PASSWORD             ""
#define THINGSBOARD_SERVER        "thingsboard.cloud"
#define THINGSBOARD_ACCESSTOKEN   "VBT8BgbDc213Rxf7RZfU" //ganti token nya di sesuaikan dengan device nya

#define SERIAL_DEBUG_BAUD    115200
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Baud rate for debug serial
#define SERIAL_DEBUG_BAUD   115200
#define Led_red 19 //Indikator Tambak Panas
#define Led_green 5 //Indikator Tambak Dingin
#define Led_yellow 18 //Indikator Tambak Normal

#define ECHO_PIN 33
#define TRIG_PIN 25

const int servoPin1 = 14; //Pompa Air Panas
const int servoPin2 = 12; //Pompa Air Dingin
const int servoPin3 = 27; //Pompa Air Pembuangan

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

DHTesp dht;
#define DHT_PIN 15

// Main application loop delay
int quant = 20;
// Initial period of LED cycling.
int led_delay = 1000;
// Period of sending a temperature/humidity data.
int send_delay = 2000;

// Time passed after LED was turned ON, milliseconds.
int led_passed = 0;
// Time passed after temperature/humidity data was sent, milliseconds.
int send_passed = 0;
int ldr_passed =0;

// Set to true if application is subscribed for the RPC messages.
bool subscribed = false;
// LED number that is currenlty ON.
int current_led = 0;
int ldrStatus;
float temperature = 0;
float humidity = 0;

RPC_Response processDelayChange(const RPC_Data &data)
{
  Serial.println("Received the set delay RPC method");

  // Process data

  led_delay = data;

  Serial.print("Set new delay: ");
  Serial.println(led_delay);

  return String(led_delay);
}

const unsigned char logoudang [] PROGMEM = {
  // Logo Udang
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x38, 0xfe, 0x00, 0xe3, 0xff, 0xf0, 0x00, 0x0c, 0x00, 0x3f, 0xff, 0xf8, 0x1f, 0xe0, 0x7f, 0xfe, 
	0x38, 0xff, 0xc0, 0xe3, 0xff, 0xf8, 0x01, 0x8c, 0x20, 0x3f, 0xff, 0xf8, 0x3f, 0xf8, 0x7f, 0xfe, 
	0x38, 0xc7, 0xe0, 0xe3, 0x80, 0x3c, 0x03, 0x8c, 0x30, 0x30, 0x00, 0x38, 0x78, 0x3c, 0x01, 0x80, 
	0x38, 0xc0, 0xf8, 0xe3, 0x80, 0x0e, 0x07, 0x8c, 0x38, 0x30, 0x00, 0x38, 0xe0, 0x0e, 0x01, 0x80, 
	0x38, 0xc0, 0x3c, 0xe3, 0x80, 0x07, 0x07, 0x0c, 0x38, 0x30, 0x00, 0x71, 0xc0, 0x07, 0x01, 0x80, 
	0x38, 0xc0, 0x1e, 0xe3, 0x80, 0x03, 0x0e, 0x0c, 0x1c, 0x30, 0x00, 0xf1, 0x80, 0x07, 0x01, 0x80, 
	0x38, 0xc0, 0x0f, 0xe3, 0x80, 0x03, 0x8e, 0x0c, 0x0c, 0x30, 0x01, 0xe3, 0x80, 0x03, 0x81, 0x80, 
	0x38, 0xc0, 0x07, 0xe3, 0x80, 0x03, 0x8c, 0x0c, 0x0e, 0x30, 0x07, 0xc3, 0x80, 0x03, 0x81, 0x80, 
	0x38, 0xc0, 0x03, 0xe3, 0x80, 0x01, 0x8c, 0x0c, 0x0e, 0x3f, 0xff, 0x83, 0x00, 0x03, 0x81, 0x80, 
	0x38, 0xc0, 0x03, 0xe3, 0x80, 0x01, 0x8c, 0x0c, 0x0e, 0x3f, 0xff, 0x83, 0x00, 0x01, 0x81, 0x80, 
	0x38, 0xc0, 0x01, 0xe3, 0x80, 0x01, 0x8c, 0x00, 0x0e, 0x38, 0x0f, 0xc3, 0x80, 0x03, 0x81, 0x80, 
	0x38, 0xc0, 0x01, 0xe3, 0x80, 0x03, 0x8e, 0x00, 0x0c, 0x30, 0x01, 0xe3, 0x80, 0x03, 0x81, 0x80, 
	0x38, 0xc0, 0x00, 0xe3, 0x80, 0x03, 0x0e, 0x00, 0x1c, 0x30, 0x00, 0xf1, 0x80, 0x03, 0x01, 0x80, 
	0x38, 0xc0, 0x00, 0xe3, 0x80, 0x07, 0x07, 0x00, 0x1c, 0x30, 0x00, 0x71, 0xc0, 0x07, 0x01, 0x80, 
	0x38, 0xc0, 0x00, 0xe3, 0x80, 0x0e, 0x07, 0x80, 0x38, 0x30, 0x00, 0x30, 0xe0, 0x0e, 0x01, 0x80, 
	0x38, 0xc0, 0x00, 0xe3, 0x80, 0x1e, 0x03, 0xc0, 0xf0, 0x30, 0x00, 0x38, 0xf0, 0x3e, 0x01, 0x80, 
	0x38, 0xc0, 0x00, 0xe3, 0xff, 0xfc, 0x01, 0xff, 0xe0, 0x3f, 0xff, 0xf8, 0x7f, 0xf8, 0x01, 0x80, 
	0x38, 0xc0, 0x00, 0xe3, 0xff, 0xf0, 0x00, 0x7f, 0xc0, 0x3f, 0xff, 0xf8, 0x1f, 0xf0, 0x01, 0x80, 
	0x38, 0xc0, 0x00, 0xe3, 0xff, 0x80, 0x00, 0x0e, 0x00, 0x3f, 0xff, 0xf8, 0x03, 0x80, 0x01, 0x80, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x07, 0x00, 0x03, 0xfc, 0x00, 0x38, 0x00, 0x3f, 0xc0, 0x03, 0xfc, 0x01, 0x80, 0x70, 0x0c, 0x0c, 
	0x07, 0x80, 0x07, 0xfc, 0x00, 0x7c, 0x00, 0x3f, 0xe0, 0x03, 0xfc, 0x01, 0xc0, 0xf0, 0x0e, 0x18, 
	0x0d, 0x80, 0x06, 0x00, 0x00, 0x6c, 0x00, 0x30, 0x70, 0x03, 0x00, 0x01, 0xc0, 0xf0, 0x07, 0x18, 
	0x0d, 0xc0, 0x0e, 0x00, 0x00, 0x6c, 0x00, 0x30, 0x38, 0x03, 0x00, 0x01, 0xe1, 0xf0, 0x03, 0x30, 
	0x1c, 0xc0, 0x0c, 0x00, 0x00, 0xc6, 0x00, 0x30, 0x38, 0x03, 0xf8, 0x01, 0xf1, 0xf0, 0x03, 0xf0, 
	0x18, 0xe0, 0x0c, 0x00, 0x00, 0xc6, 0x00, 0x30, 0x18, 0x03, 0xf8, 0x01, 0xb3, 0x70, 0x01, 0xe0, 
	0x38, 0xe0, 0x0c, 0x00, 0x01, 0xc7, 0x00, 0x30, 0x38, 0x03, 0x00, 0x01, 0x9f, 0x70, 0x01, 0xc0, 
	0x3f, 0xe0, 0x0e, 0x00, 0x01, 0xff, 0x00, 0x30, 0x30, 0x03, 0x00, 0x01, 0x9e, 0x70, 0x00, 0xc0, 
	0x30, 0x70, 0x07, 0x0c, 0x03, 0x83, 0x80, 0x30, 0x70, 0x03, 0x00, 0x01, 0x8c, 0x70, 0x00, 0xc0, 
	0x60, 0x30, 0x03, 0xfc, 0x03, 0x01, 0x80, 0x3f, 0xe0, 0x03, 0xfc, 0x01, 0x84, 0x70, 0x00, 0xc0, 
	0x60, 0x38, 0x01, 0xf8, 0x07, 0x01, 0xc0, 0x3f, 0x80, 0x03, 0xfc, 0x01, 0x80, 0x70, 0x00, 0xc0, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x62, 0xc6, 0x45, 0xd9, 0x72, 0x84, 0x06, 0x63, 0x06, 0x4d, 0x89, 0x03, 0x60, 0x49, 0xb7, 0x68, 
	0x02, 0x84, 0x08, 0x82, 0x22, 0x80, 0x80, 0x82, 0x00, 0x81, 0x41, 0x84, 0x02, 0x50, 0x04, 0x12, 
	0x0a, 0x84, 0x22, 0x80, 0xa0, 0x6a, 0x00, 0x92, 0x00, 0x81, 0x81, 0x44, 0x01, 0x54, 0x84, 0x64, 
	0x02, 0x80, 0x00, 0x92, 0x01, 0x00, 0x00, 0x62, 0x20, 0x60, 0x09, 0x03, 0x60, 0x40, 0x84, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
void setup() {
  servo1.attach(servoPin1, 500, 2400);
  servo2.attach(servoPin2, 500, 2400);
  servo3.attach(servoPin3, 500, 2400);
  
  pinMode(Led_red, OUTPUT);
  pinMode(Led_green, OUTPUT);
  pinMode(Led_yellow, OUTPUT);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  Serial.begin(SERIAL_DEBUG_BAUD);
  WiFi.begin(WIFI_AP_NAME, WIFI_PASSWORD);
  InitWiFi();
  // Initialize temperature sensor
  dht.setup(DHT_PIN, DHTesp::DHT22);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 
  display.clearDisplay();
  display.drawBitmap(0, 0, logoudang, 128, 64, WHITE);
  display.display();
}

void loop() 
{
  //delay(1000);
  delay(quant);
  send_passed += quant;
  ldr_passed += quant;
  float distance = readDistanceCM();
  float realdistance = 400 - distance;

  // Reconnect to WiFi, if needed
  if (WiFi.status() != WL_CONNECTED) {
    reconnect();
    return;
  }

  // Reconnect to ThingsBoard, if needed
  if (!tb.connected()) {
    subscribed = false;

    // Connect to the ThingsBoard
    Serial.print("Connecting to: ");
    Serial.print(THINGSBOARD_SERVER);
    Serial.print(" with token ");
    Serial.println(THINGSBOARD_ACCESSTOKEN);
    if (!tb.connect(THINGSBOARD_SERVER, THINGSBOARD_ACCESSTOKEN)) {
      Serial.println("Failed to connect");
      return;
    }
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(1);

  }

    // Check if it is a time to send DHT22 temperature and humidity
  if (send_passed > send_delay) {
    Serial.println();
    Serial.println("Sending data... ");
    TempAndHumidity lastValues = dht.getTempAndHumidity();    
    Serial.print("Tinggi kolam: ");
    Serial.print(realdistance);
    Serial.println(" cm");

    if (isnan(lastValues.humidity) || isnan(lastValues.temperature)) {
      Serial.println("Failed to read from DHT sensor!");
    } 
    else {
      temperature = lastValues.temperature;
      Serial.print("temperature: ");
      Serial.print(temperature);
      Serial.print(" degC");
      humidity = lastValues.humidity;
      Serial.print(" humidity: ");
      Serial.print(humidity);
      Serial.println(" %");
      tb.sendTelemetryFloat("temperature", temperature);
      tb.sendTelemetryFloat("humidity", humidity);
      display.setCursor(0,0);
      display.println("Temp : " + String(temperature, 1) + " C");
      display.display();
    }
  send_passed = 0;

  }
  // Menggerakkan Servo
  //Suhu Panas
  if (ldr_passed > send_delay){
    if (temperature >= 34) {
    overfill();
    servo2.write(180);
    servo1.write(0);              
    delay(1000);
    Serial.println("Suhu Panas, Pompa Air Dingin Menyala :");
    tb.sendTelemetryString("Kondisi Air", "Panas");
    digitalWrite(Led_red, HIGH);
    digitalWrite(Led_green, LOW);
    digitalWrite(Led_yellow, LOW);
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("Temp : " + String(temperature, 1) + " C");
    display.setCursor(0,30);
    display.println("Suhu Panas");
    display.setCursor(0,45);
    display.println("Pompa Air Dingin Menyala");
    display.setCursor(0,15);
    display.println("Ketinggian air" + String(realdistance, 0) + " cm");
    display.display();
  }

  //Suhu Dingin
    else if (temperature < 26){
    overfill();
    if (distance <= 250) {
      servo2.write(0); 
    }
    servo1.write(180);
    Serial.println("Suhu Dingin, Pompa Air Panas Menyala ");
    tb.sendTelemetryString("Kondisi Air", "Dingin");
    digitalWrite(Led_red, LOW);
    digitalWrite(Led_green, HIGH);
    digitalWrite(Led_yellow, LOW);
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("Temp : " + String(temperature, 1) + " C");
    display.setCursor(0,30);
    display.println("Suhu Dingin");
    display.setCursor(0,45);
    display.println("Pompa Air Panas Menyala ");
    display.setCursor(0,15);
    display.println("Ketinggian air" + String(realdistance, 0) + " cm");
    display.display();
  }

  //Suhu Sedang
    else {
    overfill();
    servo1.write(0);
    if (distance <= 250) {
      servo2.write(0);
    }
    Serial.println("Suhu Normal, Pompa Pengatur Suhu Mati ");
    tb.sendTelemetryString("Kondisi Air", "Normal");
    digitalWrite(Led_green, LOW);
    digitalWrite(Led_red, LOW);
    digitalWrite(Led_yellow, HIGH);
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("Temp : " + String(temperature, 1) + " C");
    display.setCursor(0,30);
    display.println("Suhu Normal");
    display.setCursor(0,45);
    display.println("Pompa Pengatur Suhu Mati");
    display.setCursor(0,15);
    display.println("Tinggi Air : " + String(realdistance, 0) + " cm");
    display.display();
  }
  ldr_passed = 0;
  }
  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");
}

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

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

void overfill() {
  float distance = readDistanceCM();
  // Pompa Exhaust untuk Overfill
  if (distance < 200) {
    servo3.write(180);
    servo2.write(0);
    tb.sendTelemetryString("Ketinggian Air", "Dalam");
  }
  else if (distance > 250) {
    servo3.write(0);
    servo2.write(180);
    tb.sendTelemetryString("Ketinggian Air", "Surut");
  }
  else{
    servo3.write(0);
    servo2.write(0);
    tb.sendTelemetryString("Ketinggian Air", "Normal");
  }
}