#include <WiFi.h>
#include <WiFiUdp.h>
#include <PubSubClient.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <RTClib.h>
#include <NTPClient.h>
#include <TimeLib.h>

const char* ssid = "SKK - STUDENT";
const char* password = "sistemkomputerkontrol";

const char* mqtt_server = "54.251.81.137";

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;

#define SERVO1_PIN 19
#define SERVO2_PIN 17
#define SERVO3_PIN 16
#define SERVO4_PIN 25
#define SERVO5_PIN 26

WiFiClient espClient;
PubSubClient client(espClient);

long lastMsg = 0;
char msg[500];
int value = 0;

#define relay1 13

const int trigPin = 5;
const int echoPin = 18;

#define SOUND_VELOCITY 0.034

long duration;
float distanceCm;



const int penyaluranOnHour = 4;
const int penyaluranOnMinute = 0;
const int penyaluranOffHour = 23;
const int penyaluranOffMinute = 0;

int currentMinute;
int currentHour;

const char* statusservo1 = "esp32/servo1";
const char* statusservo2 = "esp32/servo2";
const char* statusservo3 = "esp32/servo3";
const char* statusservo4 = "esp32/servo4";
const char* statusservo5 = "esp32/servo5";
const char* statusrelay1 = "esp32/relay1";

const char* statusmode = "esp32/mode";
bool autoMode = true;

String servo1Status = "off";
String servo2Status = "off";
String servo3Status = "off";
String servo4Status = "off";
String servo5Status = "off";
String relay1Status = "off";

RTC_DS3231 rtc;
WiFiUDP ntpUDP;
NTPClient timeClient(ntpUDP, "id.pool.ntp.org", 25200, 60000);  // 25200 seconds is UTC+7 for WIB

String ntpTimeString;
String rtcTimeString;

bool rtcAvailable = true;


void wifiConfig() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.print("WiFi connected - ESP32 client IP address: ");
  Serial.println(WiFi.localIP());
}

void callback(char* topic, byte* message, unsigned int length) {
  Serial.print("Message arrived on topic: ");
  Serial.print(topic);
  Serial.print(". Message: ");
  String messageTemp;

  for (int i = 0; i < length; i++) {
    Serial.print((char)message[i]);
    messageTemp += (char)message[i];
  }
  Serial.println();

  if (strcmp(topic, statusmode) == 0) {
    if (messageTemp == "auto" && !autoMode) {
      autoMode = true;
      client.publish(statusmode, "auto");
    } else if (messageTemp == "manual" && autoMode) {
      autoMode = false;
      client.publish(statusmode, "manual");

      digitalWrite(relay1, HIGH);
      digitalWrite(4, LOW);
      client.publish(statusrelay1, "off");
    }
  }

  if (!autoMode) {
    kontrolAirManual(topic, messageTemp);
  }
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    if (client.connect("ESP32Client")) {
      Serial.println("connected");
      client.subscribe("switch/servo1");
      client.subscribe("switch/servo2");
      client.subscribe("switch/servo3");
      client.subscribe("switch/servo4");
      client.subscribe("switch/servo5");
      client.subscribe("switch/relay1");
      client.subscribe(statusmode);
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void setup() {
  pinMode(12, OUTPUT);
  pinMode(14, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(15, OUTPUT);
  pinMode(2, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(relay1, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servo3.attach(SERVO3_PIN);
  servo4.attach(SERVO4_PIN);
  servo5.attach(SERVO5_PIN);

  digitalWrite(12, LOW);
  digitalWrite(14, LOW);
  digitalWrite(27, LOW);
  digitalWrite(2, LOW);
  digitalWrite(4, LOW);
  digitalWrite(15, LOW);
  digitalWrite(relay1, HIGH);

  Serial.begin(9600);
  wifiConfig();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);

  Wire.begin(21, 22);
}

void loop() {
  if (WiFi.status() == WL_CONNECTED) {
    if (!client.connected()) {
      reconnect();
    }
    client.loop();
    if (!rtc.begin()) {
      Serial.println("Couldn't find RTC");
      rtcAvailable = false;
    }
    timeClient.begin();                                    // Start NTP client
    timeClient.update();                                   // Retrieve current epoch time from NTP server
    unsigned long unix_epoch = timeClient.getEpochTime();  // Get epoch t
    if (rtcAvailable) {
      rtc.adjust(DateTime(unix_epoch));  // Set RTC time using NTP epoch time
    } else {
      setTime(unix_epoch);  // Set system time using NTP epoch time
    }
  } else {
    Serial.println("WiFi not connected. Running in offline mode.");
  }
  getJarak();
  long now = millis();
  if (now - lastMsg > 2000) {
    lastMsg = now;
    if (autoMode) {
      kontrolAirAuto();
    }
  }
  getTime();
}

void getTime() {
  timeClient.update();                                   
  unsigned long unix_epoch = timeClient.getEpochTime();  

  byte second_ = second(unix_epoch);  
  currentMinute = minute(unix_epoch); 
  currentHour = hour(unix_epoch);     
  byte day_ = day(unix_epoch);        
  byte month_ = month(unix_epoch);    
  int year_ = year(unix_epoch);       
  ntpTimeString = String(currentHour) + ":" + String(currentMinute) + ":" + String(second_) + " " + String(day_) + "/" + String(month_) + "/" + String(year_);
  client.publish("time/ntp", ntpTimeString.c_str());

  if (rtcAvailable) {
    DateTime rtcTime = rtc.now();  
    rtcTimeString = String(rtcTime.hour()) + ":" + String(rtcTime.minute()) + ":" + String(rtcTime.second()) + " " + String(rtcTime.day()) + "/" + String(rtcTime.month()) + "/" + String(rtcTime.year());
    if (rtcTime != DateTime(year_, month_, day_, currentHour, currentMinute, second_)) {
      rtc.adjust(DateTime(unix_epoch));  // Adjust RTC time if not synchronized
      Serial.println("RTC time adjusted to NTP time");
    } else {
      Serial.println("Time is synchronized!");  // Print synchronization status
    }
  }

  Serial.println("NTP Time: " + ntpTimeString);
  if (rtcAvailable) {
    Serial.println("RTC Time: " + rtcTimeString);
    client.publish("time/rtc", rtcTimeString.c_str());
  }
}

void kontrolAirAuto() {
  pengisianAir(distanceCm);
  kontrolPenampunganAir(distanceCm);
}

void kontrolAirManual(String topic, String messageTemp) {
  if (topic == "switch/servo1") {
    if (messageTemp != servo1Status) {
      Serial.print("Penyaluran Air 1: ");
      if (messageTemp == "on") {
        servo1.write(60);
        digitalWrite(12, HIGH);
        Serial.println("On");
      } else if (messageTemp == "off") {
        servo1.write(0);
        digitalWrite(12, LOW);
        Serial.println("Off");
      }
      servo1Status = messageTemp;
    }
  }

  if (topic == "switch/servo2") {
    if (messageTemp != servo2Status) {
      Serial.print("Penyaluran Air 2: ");
      if (messageTemp == "on") {
        servo2.write(60);
        digitalWrite(14, HIGH);
        Serial.println("On");
      } else if (messageTemp == "off") {
        servo2.write(0);
        digitalWrite(14, LOW);
        Serial.println("Off");
      }
      servo2Status = messageTemp;
    }
  }

  if (topic == "switch/servo3") {
    if (messageTemp != servo3Status) {
      Serial.print("Penyaluran Air 3: ");
      if (messageTemp == "on") {
        servo3.write(60);
        digitalWrite(27, HIGH);
        Serial.println("On");
      } else if (messageTemp == "off") {
        servo3.write(0);
        digitalWrite(27, LOW);
        Serial.println("Off");
      }
      servo3Status = messageTemp;
    }
  }

  if (topic == "switch/servo4") {
    if (messageTemp != servo4Status) {
      Serial.print("Penyaluran Air 4: ");
      if (messageTemp == "on") {
        servo4.write(60);
        digitalWrite(15, HIGH);
        Serial.println("On");
      } else if (messageTemp == "off") {
        servo4.write(0);
        digitalWrite(15, LOW);
        Serial.println("Off");
      }
      servo4Status = messageTemp;
    }
  }

  if (topic == "switch/servo5") {
    if (messageTemp != servo5Status) {
      Serial.print("Penyaluran Air 5: ");
      if (messageTemp == "on") {
        servo5.write(60);
        digitalWrite(2, HIGH);
        Serial.println("On");
      } else if (messageTemp == "off") {
        servo5.write(0);
        digitalWrite(2, LOW);
        Serial.println("Off");
      }
      servo5Status = messageTemp;
    }
  }

  if (topic == "switch/relay1") {
    if (messageTemp != relay1Status) {
      Serial.print("Pengisian Air : ");
      if (messageTemp == "on") {
        digitalWrite(relay1, LOW);
        digitalWrite(4, HIGH);
        Serial.println("On");
      } else if (messageTemp == "off") {
        digitalWrite(relay1, HIGH);
        digitalWrite(4, LOW);
        Serial.println("Off");
      }
      relay1Status = messageTemp;
    }
  }
}

void getJarak() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distanceCm = duration * SOUND_VELOCITY / 2;

  Serial.print("Distance (cm): ");
  Serial.println(distanceCm);
  char jarakCm[8];
  dtostrf(distanceCm, 1, 2, jarakCm);
  client.publish("esp32/jarak", jarakCm);
  // client.publish("esp32/statusAir", "Penyaluran Air Ditutup");
}

void kontrolPenampunganAir(int distanceCm) {
  if (distanceCm == 2) {
    Serial.println("Air sudah penuh");
    kontrolPenyaluranAir(true, currentHour, currentMinute);
    client.publish("esp32/statusAir", "Penyaluran Air Dibuka");
  } else if (distanceCm > 2 && distanceCm <= 15) {
    Serial.println("Menunggu air penuh");
  } else if (distanceCm > 15) {
    Serial.println("Penyaluran Air ditutup");
    kontrolPenyaluranAir(false, currentHour, currentMinute);
    client.publish("esp32/statusAir", "Penyaluran Air Ditutup");
  }
}

void pengisianAir(int distanceCm) {
  if (distanceCm > 2 && distanceCm <= 20) {
    Serial.println("Pengisian Air dilakukan");
    digitalWrite(relay1, LOW);
    digitalWrite(4, HIGH);
    client.publish(statusrelay1, "on");
  } else if (distanceCm <= 2) {
    Serial.println("Air Sudah Penuh, Pengisian Air Berhenti");
    digitalWrite(relay1, HIGH);
    digitalWrite(4, LOW);
    client.publish(statusrelay1, "off");
  }
}

void kontrolPenyaluranAir(bool status, int currentHour, int currentMinute) {
  if (status) {
    if ((currentHour > penyaluranOnHour || (currentHour == penyaluranOnHour && currentMinute >= penyaluranOnMinute)) && (currentHour < penyaluranOffHour || (currentHour == penyaluranOffHour && currentMinute < penyaluranOffMinute))) {
      Serial.println("Jadwal Penyaluran Air dibuka");
      servo1.write(60);
      digitalWrite(12, HIGH);
      Serial.println("Servo1 Hidup");
      client.publish(statusservo1, "on");
      servo2.write(60);
      digitalWrite(14, HIGH);
      Serial.println("Servo2 Hidup");
      client.publish(statusservo2, "on");
      servo3.write(60);
      digitalWrite(27, HIGH);
      Serial.println("Servo3 Hidup");
      client.publish(statusservo3, "on");
      servo4.write(60);
      digitalWrite(15, HIGH);
      Serial.println("Servo4 Hidup");
      client.publish(statusservo4, "on");
      servo5.write(60);
      digitalWrite(2, HIGH);
      Serial.println("Servo5 Hidup");
      client.publish(statusservo5, "on");
    } else {
      Serial.println("Jadwal Penyaluran Air ditutup");
      servo1.write(0);
      digitalWrite(12, LOW);
      Serial.println("Servo1 Mati");
      client.publish(statusservo1, "off");
      servo2.write(0);
      digitalWrite(14, LOW);
      Serial.println("Servo2 Mati");
      client.publish(statusservo2, "off");
      servo3.write(0);
      digitalWrite(27, LOW);
      Serial.println("Servo3 Mati");
      client.publish(statusservo3, "off");
      servo4.write(0);
      digitalWrite(15, LOW);
      Serial.println("Servo4 Mati");
      client.publish(statusservo4, "off");
      servo5.write(0);
      digitalWrite(2, LOW);
      Serial.println("Servo5 Mati");
      client.publish(statusservo5, "off");
    }
  } else {
    Serial.println("Menunggu Air bisa disalurkan");
    servo1.write(0);
    digitalWrite(12, LOW);
    Serial.println("Servo1 Mati");
    client.publish(statusservo1, "off");
    servo2.write(0);
    digitalWrite(14, LOW);
    Serial.println("Servo2 Mati");
    client.publish(statusservo2, "off");
    servo3.write(0);
    digitalWrite(27, LOW);
    Serial.println("Servo3 Mati");
    client.publish(statusservo3, "off");
    servo4.write(0);
    digitalWrite(15, LOW);
    Serial.println("Servo4 Mati");
    client.publish(statusservo4, "off");
    servo5.write(0);
    digitalWrite(2, LOW);
    Serial.println("Servo5 Mati");
    client.publish(statusservo5, "off");
  }
}