#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <ezButton.h>
#include <TaskScheduler.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>
#include <Preferences.h>

Preferences preferences;

byte iup[] = {B00000, B00100, B01010, B10001, B00100, B01010, B10001, B00000};
byte idown[] = {B00000, B10001, B01010, B00100, B10001, B01010, B00100, B00000};
byte istop[] = {B00000, B11111, B11111, B11111, B11111, B11111, B00000, B00000};
byte ifeedStart[] = {B11111, B11111, B01110, B01110, B00000, B00100, B00000, B00100};
byte ifeedStop[] = {B11111, B10001, B01010, B01110, B00000, B00000, B00000, B00000};
byte ilimitUp[] = {B00100, B01110, B10101, B00100, B11111, B11111, B00000, B00000};
byte ilimitDown[] = {B00100, B00100, B00100, B00100, B00100, B00100, B11111, B11111};
byte isignalOn[] = {B00001, B00011, B00111, B01111, B11111, B11111, B00000, B00000};
byte isignalOff[] = {B10100, B01000, B10101, B00011, B00011, B00111, B01111, B11111};

// Inisialisasi LCD (alamat I2C, jumlah kolom, jumlah baris)
const int lcdColumns = 16;
const int lcdRows = 2;
LiquidCrystal_I2C lcd(0x27, lcdColumns, lcdRows);

// Variabel untuk deteksi long press pada tombol stop
unsigned long stopButtonPressStartTime = 0;
bool isStopButtonLongPressed = false;
const unsigned long LONG_PRESS_DURATION = 3000; // 3 detik


// MQTT dan WiFi credentials
const char* ssid = "IOT-EKASA";
const char* password = "iot-ekasa";
const char* mqtt_broker = "server.local";
const int mqtt_port = 1883;
const char* mqttUser = "";
const char* mqttPassword = "";

// MQTT topic
const char* mqttTopicSetOtomatis = "setOtomatis";
const char* mqttTopicKontrol = "kontrol";

// WiFi dan MQTT Client
WiFiClient espClient;
PubSubClient client(espClient);




// Deklarasi Fungsi
void wifi_connect();
void mqtt_connect();
void mqttCallback(char* topic, byte* payload, unsigned int length);

void updateTimeLeft();
void checkButtons();
void stopAll();
void startProcess(const char *status, unsigned long duration);
void setIconUp();
void setIconDown();
void setIconStop();
bool checkTimeElapsed(unsigned long duration);
void checkLimitSwitch();
void checkConnections();

// Scheduler dan Tugas
Scheduler runner;
Task taskUpdateTime(500, TASK_FOREVER, &updateTimeLeft);
Task taskCheckButtons(100, TASK_FOREVER, &checkButtons);
Task taskCheckLimitSwitch(50, TASK_FOREVER, &checkLimitSwitch);
Task taskCheckConnections(1000, TASK_FOREVER, &checkConnections); // Setiap 1000 ms (1 detik)

// Variabel
Servo myServo;
int servoPin = 4;
const int relay1 = 32;
const int relay2 = 33;
const int limitSwitchPin = 5;
// Tambahkan variabel global untuk menyimpan status sebelumnya
bool previousRemoteMode = false;

bool standbyActive = false;
unsigned long standbyStartTime = 0;
const unsigned long standbyDuration = 5000; // 5 detik

// Tombol dan LED
const int selectorButtonPin = 25;  // Selector mode (Manual / Otomatis)
const int ledPin = 26;  // LED untuk indikasi mode



  
// Tombol menggunakan ezButton
ezButton buttonCW(13);  // Tombol UP
ezButton buttonCCW(12); // Tombol DOWN
ezButton buttonServo(27);
ezButton btnEmergencyStop(14);



unsigned long durasiServo = 5;
unsigned long durasiTurun = 5;
unsigned long durasiRendam = 1;  // Dalam menit
unsigned long startTime = 0;

bool servoActive = false;
bool turunActive = false;
bool rendamActive = false;
bool upActive = false;
bool processActive = false;
bool isManualMode = true;  // Default: Mode Manual
bool remoteMode = false;  // Default: Remote mode = false (Manual Mode)

// Variabel debounce untuk Selector Mode
unsigned long lastDebounceTimeSelector = 0;
const unsigned long debounceDelaySelector = 500;

void setup() {
  Serial.begin(115200);
  lcd.init();
  lcd.backlight();
  lcd.createChar(0, ifeedStart);
  lcd.createChar(1, ifeedStop);
  lcd.createChar(2, iup);
  lcd.createChar(3, idown);
  lcd.createChar(4, istop);
  lcd.createChar(5, ilimitUp);
  lcd.createChar(6, ilimitDown);
  lcd.createChar(7, isignalOn);
  
  // Setup tombol dengan debounce
  buttonCW.setDebounceTime(50);
  buttonCCW.setDebounceTime(50);
  buttonServo.setDebounceTime(50);
  btnEmergencyStop.setDebounceTime(50);

  pinMode(limitSwitchPin, INPUT_PULLUP);
  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);
  pinMode(selectorButtonPin, INPUT_PULLUP);  // Selector Mode
  pinMode(ledPin, OUTPUT);  // LED Indikator
  
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  myServo.attach(servoPin, 500, 2400);
  myServo.write(0);

  // Setup TaskScheduler
  runner.init();
  runner.addTask(taskUpdateTime);
  runner.addTask(taskCheckButtons);
  runner.addTask(taskCheckLimitSwitch);
  runner.addTask(taskCheckConnections);
  taskUpdateTime.enable();
  taskCheckButtons.enable();
  taskCheckLimitSwitch.enable();
  taskCheckConnections.enable();
  setSignalOff();

//  clearKeterangan();
//  wifi_connect();
//  mqtt_connect();

// Inisialisasi Preferences
  preferences.begin("config", false); // Namespace "config", read-write mode

  // Baca durasiServo, durasiTurun, durasiRendam dari Preferences
  durasiServo = preferences.getUInt("durasiServo", 5); // Default 5 detik
  durasiTurun = preferences.getUInt("durasiTurun", 5); // Default 5 detik
  durasiRendam = preferences.getUInt("durasiRendam", 1); // Default 1 menit

  Serial.println("Durasi Servo: " + String(durasiServo) + " detik");
  Serial.println("Durasi Turun: " + String(durasiTurun) + " detik");
  Serial.println("Durasi Rendam: " + String(durasiRendam) + " menit");
  // Tampilkan informasi timer pertama kali
  displayInfoTimer();

      
   if (digitalRead(limitSwitchPin) == LOW) {
//    lcd.clear();
    clearKeterangan();
    lcd.setCursor(2, 0);
    lcd.print("Anco diatas");
    lcd.setCursor(2, 1);
    lcd.print("Standby ...");
    
    
    
  }else{
//    lcd.clear();
    clearKeterangan();
    setLimitUp();
    lcd.setCursor(2, 0);
    lcd.print("Set Posisi!");
    lcd.setCursor(2, 1);
    lcd.print("Ready");    
  }
  
  delay(1000);

   // Pengecekan Selector Mode (Remote Mode)
  if (digitalRead(selectorButtonPin) == LOW) {
    remoteMode = true;  // Aktifkan Remote Mode
    digitalWrite(ledPin, HIGH);  // Nyalakan LED
    lcd.setCursor(14, 0);
    lcd.print("|R");
    delay(500);  // Debounce sederhana
  } else {
    remoteMode = false;  // Nonaktifkan Remote Mode (Manual Mode)
    digitalWrite(ledPin, LOW);  // Matikan LED
    lcd.setCursor(14, 0);
    lcd.print("|M");
  }
  delay (1000);
}

void loop() {
  runner.execute();  // Jalankan scheduler
  client.loop();  // MQTT loop
 }

// Fungsi untuk memperbarui waktu tersisa
void updateTimeLeft() {
  if (processActive) {
    unsigned long elapsedTime = (millis() - startTime) / 1000;
    unsigned long timeLeft = 0;

    if (servoActive) {
      timeLeft = max(0UL, durasiServo - elapsedTime);
    } else if (turunActive) {
      timeLeft = max(0UL, durasiTurun - elapsedTime);
    } else if (rendamActive) {
      timeLeft = max(0UL, (durasiRendam * 60) - elapsedTime);
    }

    unsigned long hours = timeLeft / 3600;
    unsigned long minutes = (timeLeft % 3600) / 60;
    unsigned long seconds = timeLeft % 60;

    lcd.setCursor(2, 1);
    lcd.print("Time: ");
    if (hours < 10) lcd.print('0');
    lcd.print(hours);
    lcd.print(':');
    if (minutes < 10) lcd.print('0');
    lcd.print(minutes);
    lcd.print(':');
    if (seconds < 10) lcd.print('0');
    lcd.print(seconds);
  }
}

//// Fungsi untuk memantau tombol
//void checkButtons() {
//  buttonCW.loop();
//  buttonCCW.loop();
//  buttonServo.loop();
//  btnEmergencyStop.loop();
//
//
//  // Pengecekan Selector Mode (Remote Mode)
//  if (digitalRead(selectorButtonPin) == LOW) {
//    remoteMode = true;  // Aktifkan Remote Mode
//    digitalWrite(ledPin, HIGH);  // Nyalakan LED
//    lcd.setCursor(14, 0);
//    lcd.print("|R");
//    delay(500);  // Debounce sederhana
//  } else {
//    remoteMode = false;  // Nonaktifkan Remote Mode (Manual Mode)
//    digitalWrite(ledPin, LOW);  // Matikan LED
//    lcd.setCursor(14, 0);
//    lcd.print("|M");
//    disconnectWiFi();
//  }
//  
//  if (btnEmergencyStop.isPressed()) {
//    stopAll();
//  }
//
//  if (buttonServo.isPressed() && !processActive) {
//    processActive = true;
//    servoActive = true;
//    startProcess("Feeding", durasiServo);
//    setFeedStart();
//    myServo.write(90);
//  }
//
//  if (servoActive && checkTimeElapsed(durasiServo)) {
//    clearKeterangan();
//    setFeedStop();
//    myServo.write(0);
//    lcd.setCursor(2, 0);
//    lcd.print("Stop Feed");
//    lcd.setCursor(2, 1);
//    lcd.print("Standby ...(5s)");
//    delay(5000);
//    servoActive = false;
//    turunActive = true;
////    lcd.clear();
//    clearKeterangan();
//    startProcess("Turun", durasiTurun);
//    setIconDown();
//    digitalWrite(relay1, LOW);
//    digitalWrite(relay2, HIGH);
//  }
//
//  if (turunActive && checkTimeElapsed(durasiTurun)) {
//    turunActive = false;
//    rendamActive = true;
//    
//    startProcess("Rendam", durasiRendam * 60);
//    setIconStop();
//    digitalWrite(relay1, LOW);
//    digitalWrite(relay2, LOW);
//  }
//
//  if (rendamActive && checkTimeElapsed(durasiRendam * 60)) {
//    rendamActive = false;
//    upActive = true;
//    setIconUp();
////    startProcess("Anco Naik", 0);
//    clearKeterangan();
//    lcd.setCursor(2, 0);
//    lcd.print("Angkat Anco");
//    lcd.setCursor(2, 1);
//    lcd.print("Motor Up");
//    digitalWrite(relay1, HIGH);
//    digitalWrite(relay2, LOW);
//  }
//if (!remoteMode) {  // Kontrol Manual
//  if (buttonCW.isPressed()) {
//    clearKeterangan();
//    lcd.setCursor(2, 0);
//    lcd.print("Instruksi:");
//    lcd.setCursor(2, 1);
//    lcd.print("Anco Naik");
//    setIconUp();
//    upActive = true;
//    digitalWrite(relay1, HIGH);
//    digitalWrite(relay2, LOW);
//  }
//
//  if (buttonCCW.isPressed()) {
//    clearKeterangan();
//    lcd.setCursor(2, 0);
//    lcd.print("Instruksi:");
//    lcd.setCursor(2, 1);
//    lcd.print("Anco Turun"); 
//    setIconDown();
//    digitalWrite(relay1, LOW);
//    digitalWrite(relay2, HIGH);
//    }
//  }
//}


//void checkButtons() {
//  buttonCW.loop();
//  buttonCCW.loop();
//  buttonServo.loop();
//  btnEmergencyStop.loop();
//
//  unsigned long currentMillis = millis();
//
//  // Pengecekan Selector Mode (Remote Mode) dengan debounce non-blocking
//  bool currentSelectorState = (digitalRead(selectorButtonPin) == LOW);
//  if (currentSelectorState != previousRemoteMode) {
//    if (currentMillis - lastDebounceTimeSelector > debounceDelaySelector) {
//      if (currentSelectorState) {
//        // Mode berubah ke Remote Mode
//        remoteMode = true;
//        digitalWrite(ledPin, HIGH);  // Nyalakan LED
//        lcd.setCursor(14, 0);
//        lcd.print("|R ");
//        setSignalOn(); // Update ikon koneksi
//        Serial.println("Mode: Remote");
//      } else {
//        // Mode berubah ke Manual Mode
//        remoteMode = false;
//        digitalWrite(ledPin, LOW);  // Matikan LED
//        lcd.setCursor(14, 0);
//        lcd.print("|M ");
//        disconnectWiFi(); // Putuskan koneksi WiFi dan MQTT
//        Serial.println("Mode: Manual");
//      }
//      lastDebounceTimeSelector = currentMillis;
//    }
//  }
//  previousRemoteMode = currentSelectorState;
//
//  // Pengecekan tombol Emergency Stop
//  if (btnEmergencyStop.isPressed()) {
//    stopAll();
//  }
//
//  // Pengecekan tombol Servo
//  if (buttonServo.isPressed() && !processActive && !remoteMode) {
//    processActive = true;
//    servoActive = true;
//    startProcess("Feeding", durasiServo);
//    setFeedStart();
//    myServo.write(90);
//    Serial.println("Proses manual dimulai.");
//  }
//
//  // Pengecekan durasi Servo
//  if (servoActive && checkTimeElapsed(durasiServo)) {
//    clearKeterangan();
//    setFeedStop();
//    myServo.write(0);
//    lcd.setCursor(2, 0);
//    lcd.print("Stop Feed");
//    lcd.setCursor(2, 1);
//    lcd.print("Standby ...(5s)");
//
//    // Mulai Standby tanpa delay
//    standbyActive = true;
//    standbyStartTime = millis();
//    servoActive = false;
//  }
//
//  // Pengecekan Standby
//  if (standbyActive && (millis() - standbyStartTime >= standbyDuration)) {
//    standbyActive = false;
//    turunActive = true;
//    clearKeterangan();
//    startProcess("Turun", durasiTurun);
//    setIconDown();
//    digitalWrite(relay1, LOW);
//    digitalWrite(relay2, HIGH);
//    Serial.println("Proses turun dimulai setelah standby.");
//  }
//
//  // Pengecekan durasi Turun
//  if (turunActive && checkTimeElapsed(durasiTurun)) {
//    turunActive = false;
//    rendamActive = true;
//
//    startProcess("Rendam Anco", durasiRendam * 60);
//    setIconStop();
//    digitalWrite(relay1, LOW);
//    digitalWrite(relay2, LOW);
//    Serial.println("Proses rendam dimulai.");
//  }
//
//  // Pengecekan durasi Rendam
//  if (rendamActive && checkTimeElapsed(durasiRendam * 60)) {
//    rendamActive = false;
//    upActive = true;
//    setIconUp();
//    clearKeterangan();
//    lcd.setCursor(2, 0);
//    lcd.print("Angkat Anco");
//    lcd.setCursor(2, 1);
//    lcd.print("Motor Up");
//    digitalWrite(relay1, HIGH);
//    digitalWrite(relay2, LOW);
//    Serial.println("Proses angkat anco dimulai.");
//  }
//
//  // Kontrol Manual
//  if (!remoteMode) {  // Kontrol Manual
//    if (buttonCW.isPressed()) {
//      clearKeterangan();
//      lcd.setCursor(2, 0);
//      lcd.print("Instruksi:");
//      lcd.setCursor(2, 1);
//      lcd.print("Anco Naik");
//      setIconUp();
//      upActive = true;
//      digitalWrite(relay1, HIGH);
//      digitalWrite(relay2, LOW);
//    }
//
//    if (buttonCCW.isPressed()) {
//      clearKeterangan();
//      lcd.setCursor(2, 0);
//      lcd.print("Instruksi:");
//      lcd.setCursor(2, 1);
//      lcd.print("Anco Turun"); 
//      setIconDown();
//      digitalWrite(relay1, LOW);
//      digitalWrite(relay2, HIGH);
//    }
//  }
//}

void checkButtons() {
  buttonCW.loop();
  buttonCCW.loop();
  buttonServo.loop();
  btnEmergencyStop.loop();

  unsigned long currentMillis = millis();

  // Pengecekan Selector Mode (Remote Mode) dengan debounce non-blocking
  bool currentSelectorState = (digitalRead(selectorButtonPin) == LOW);
  if (currentSelectorState != previousRemoteMode) {
    if (currentMillis - lastDebounceTimeSelector > debounceDelaySelector) {
      if (currentSelectorState) {
        // Mode berubah ke Remote Mode
        remoteMode = true;
        digitalWrite(ledPin, HIGH);  // Nyalakan LED
        lcd.setCursor(14, 0);
        lcd.print("|R ");
        setSignalOn(); // Update ikon koneksi
        Serial.println("Mode: Remote");
      } else {
        // Mode berubah ke Manual Mode
        remoteMode = false;
        digitalWrite(ledPin, LOW);  // Matikan LED
        lcd.setCursor(14, 0);
        lcd.print("|M ");
        disconnectWiFi(); // Putuskan koneksi WiFi dan MQTT
        Serial.println("Mode: Manual");
      }
      lastDebounceTimeSelector = currentMillis;
    }
  }
  previousRemoteMode = currentSelectorState;

  // Pengecekan tombol Emergency Stop dengan deteksi long press
  if (btnEmergencyStop.isPressed()) {
    if (stopButtonPressStartTime == 0) {
      // Tombol baru saja ditekan
      stopButtonPressStartTime = currentMillis;
      isStopButtonLongPressed = false;
      Serial.println("Emergency Stop button pressed.");
    } else {
      // Tombol sedang ditekan
      if (!isStopButtonLongPressed && (currentMillis - stopButtonPressStartTime >= LONG_PRESS_DURATION)) {
        // Long press terdeteksi
        isStopButtonLongPressed = true;
        displayInfoTimer();
        Serial.println("Emergency Stop button long pressed. Displaying Info Timer.");
      }
    }
  } else {
    if (stopButtonPressStartTime != 0) {
      // Tombol dilepas
      if (!isStopButtonLongPressed) {
        // Ini adalah short press
        stopAll();
        Serial.println("Emergency Stop button short pressed. Stopping all processes.");
      }
      // Reset variabel deteksi
      stopButtonPressStartTime = 0;
      isStopButtonLongPressed = false;

      // Tampilkan informasi timer setelah tombol dilepas
      displayInfoTimer();
    }
  }

  // Pengecekan tombol Servo
  if (buttonServo.isPressed() && !processActive && !remoteMode) {
    processActive = true;
    servoActive = true;
    startProcess("Feeding", durasiServo);
    setFeedStart();
    myServo.write(90);
    Serial.println("Proses manual dimulai.");
  }

  // Pengecekan durasi Servo
  if (servoActive && checkTimeElapsed(durasiServo)) {
    clearKeterangan();
    setFeedStop();
    myServo.write(0);
    lcd.setCursor(2, 0);
    lcd.print("Stop Feed");
    lcd.setCursor(2, 1);
    lcd.print("Standby ...(5s)");

    // Mulai Standby tanpa delay
    standbyActive = true;
    standbyStartTime = millis();
    servoActive = false;
  }

  // Pengecekan Standby
  if (standbyActive && (millis() - standbyStartTime >= standbyDuration)) {
    standbyActive = false;
    turunActive = true;
    clearKeterangan();
    startProcess("Turun", durasiTurun);
    setIconDown();
    digitalWrite(relay1, LOW);
    digitalWrite(relay2, HIGH);
    Serial.println("Proses turun dimulai setelah standby.");
  }

  // Pengecekan durasi Turun
  if (turunActive && checkTimeElapsed(durasiTurun)) {
    turunActive = false;
    rendamActive = true;

    startProcess("Rendam Anco", durasiRendam * 60);
    setIconStop();
    digitalWrite(relay1, LOW);
    digitalWrite(relay2, LOW);
    Serial.println("Proses rendam dimulai.");
  }

  // Pengecekan durasi Rendam
  if (rendamActive && checkTimeElapsed(durasiRendam * 60)) {
    rendamActive = false;
    upActive = true;
    setIconUp();
    clearKeterangan();
    lcd.setCursor(2, 0);
    lcd.print("Angkat Anco");
    lcd.setCursor(2, 1);
    lcd.print("Motor Up");
    digitalWrite(relay1, HIGH);
    digitalWrite(relay2, LOW);
    Serial.println("Proses angkat anco dimulai.");
  }

  // Kontrol Manual
  if (!remoteMode) {  // Kontrol Manual
    if (buttonCW.isPressed()) {
      clearKeterangan();
      lcd.setCursor(2, 0);
      lcd.print("Instruksi:");
      lcd.setCursor(2, 1);
      lcd.print("Anco Naik");
      setIconUp();
      upActive = true;
      digitalWrite(relay1, HIGH);
      digitalWrite(relay2, LOW);
    }

    if (buttonCCW.isPressed()) {
      clearKeterangan();
      lcd.setCursor(2, 0);
      lcd.print("Instruksi:");
      lcd.setCursor(2, 1);
      lcd.print("Anco Turun"); 
      setIconDown();
      digitalWrite(relay1, LOW);
      digitalWrite(relay2, HIGH);
    }
  }
}




// Fungsi pengecekan limit switch saat UP
void checkLimitSwitch() {
  if (upActive && digitalRead(limitSwitchPin) == LOW) {
    stopAll();
    setLimitUp();
    clearKeterangan();
    lcd.setCursor(2, 0);
    lcd.print("Anco di atas");
    lcd.setCursor(2, 1);
    lcd.print("Standby ...");
    
  }
}

// Fungsi untuk memulai proses
void startProcess(const char *status, unsigned long duration) {
//  lcd.clear();
  clearKeterangan();
  startTime = millis();
  lcd.setCursor(2, 0);
  lcd.print(status);
}

// Fungsi untuk menghentikan semua proses
void stopAll() {
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  myServo.write(0);
  servoActive = false;
  turunActive = false;
  rendamActive = false;
  upActive = false;
  processActive = false;
//  lcd.clear();
  clearKeterangan();
  lcd.setCursor(2, 0);
  lcd.print("Instruksi:");
  lcd.setCursor(2, 1);
  lcd.print("Motor Stop");
  setIconStop();
}

//// Fungsi untuk kontrol tampilan UP
//void setUp() {
//  lcd.clear();
//  lcd.setCursor(0, 0);
//  lcd.print("UP");
//}
//
//// Fungsi untuk kontrol tampilan DOWN
//void setDown() {
//  lcd.clear();
//  lcd.setCursor(0, 0);
//  lcd.print("DOWN");
//}

// Fungsi untuk mengecek apakah waktu habis
bool checkTimeElapsed(unsigned long duration) {
  return millis() - startTime >= duration * 1000;
}


 // Fungsi koneksi WiFi
  void wifi_connect() {
    Serial.print("Connecting to WiFi...");
//    lcd.clear();
    setSignalOff();
    clearKeterangan();
    lcd.setCursor(2, 0);
    lcd.print("WiFi status:");
    lcd.setCursor(2, 1);
    lcd.print("Connecting...");
    
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
    }
  
    Serial.println("Connected to WiFi");
//    lcd.clear();
    clearKeterangan();
    lcd.setCursor(2, 0);
    lcd.print("Wifi Connected");
    lcd.setCursor(2, 1);
    lcd.print(WiFi.localIP());
    setSignalOn();
    delay(1000);
    
  }
  
  // Fungsi koneksi MQTT
  void mqtt_connect() {
    client.setServer(mqtt_broker, mqtt_port);
    client.setCallback(mqttCallback);
  
    while (!client.connected()) {
      Serial.print("Connecting to MQTT...");
      if (client.connect("ESP32Client", mqttUser, mqttPassword)) {
        Serial.println("connected");
//        lcd.clear();
        clearKeterangan();
        lcd.setCursor(2, 0);
        lcd.print("Server Con:");
        lcd.setCursor(2, 1);
        lcd.print("Connected");
        setSignalOn();
        client.subscribe(mqttTopicSetOtomatis);
        client.subscribe(mqttTopicKontrol);
      } else {
        Serial.print("failed with state ");
        Serial.println(client.state());
//        lcd.clear();
        clearKeterangan();
        lcd.setCursor(2, 0);
        lcd.print("Net status:");
        lcd.setCursor(2, 1);
        lcd.print("Disconected");
        setSignalOff();
        delay(2000);
      }
    }
  }
  
//// Callback MQTT untuk menerima pesan
//void mqttCallback(char* topic, byte* payload, unsigned int length) {
//  String message;
//  for (int i = 0; i < length; i++) {
//    message += (char)payload[i];
//  }
//
//  Serial.print("Message received: ");
//  Serial.println(message);
//   // Parsing JSON payload
//  DynamicJsonDocument doc(1024);
//  DeserializationError error = deserializeJson(doc, payload, length);
//  if (error) {
//    Serial.print("deserializeJson() failed: ");
//    Serial.println(error.f_str());
//    return;
//  }
//  String instruksi = doc["instruksi"];
//  durasiServo = doc["durasiServo"].as<unsigned long>();
//  durasiTurun = doc["durasiTurun"].as<unsigned long>();
//  durasiRendam = doc["durasiRendam"].as<unsigned long>();
//
//  if (instruksi == "otomatis") {
//    if (!processActive) {
//      processActive = true;
//      servoActive = true;
//      startProcess("Feeding", durasiServo);
//      setFeedStart();
//      myServo.write(90);
//      Serial.println("Proses otomatis dimulai.");
//    }
//  }
//  
//  if (String(topic) == mqttTopicKontrol) {
//    if (message == "naik") {
//      digitalWrite(relay1, HIGH);
//      digitalWrite(relay2, LOW);
////      lcd.clear();
//      clearKeterangan();
//      lcd.setCursor(2, 0);
//      lcd.print("Instruksi:");
//      lcd.setCursor(2, 1);
//      lcd.print("Anco Naik");
//      setIconUp();
//    } else if (message == "turun") {
//      digitalWrite(relay1, LOW);
//      digitalWrite(relay2, HIGH);
////      lcd.clear();
//      clearKeterangan();
//      lcd.setCursor(2, 0);
//      lcd.print("Instruksi:");
//      lcd.setCursor(2, 1);
//      lcd.print("Anco Turun");
//      setIconUp();
//    } else if (message == "stop") {
//      stopAll();
////      lcd.clear();
//      clearKeterangan();
//      lcd.setCursor(2, 0);
//      lcd.print("Instruksi:");
//      lcd.setCursor(2, 1);
//      lcd.print("Motor Stop");
//      setIconStop();
//    }
//  }
//}

// Callback MQTT untuk menerima pesan
void mqttCallback(char* topic, byte* payload, unsigned int length) {
  String message;
  for (int i = 0; i < length; i++) {
    message += (char)payload[i];
  }

  Serial.print("Message received on topic [");
  Serial.print(topic);
  Serial.print("]: ");
  Serial.println(message);

  // Penanganan untuk topik "setOtomatis" dengan format JSON
  if (String(topic) == mqttTopicSetOtomatis) {
    // Parsing JSON payload
    DynamicJsonDocument doc(1024);
    DeserializationError error = deserializeJson(doc, payload, length);
    if (error) {
      Serial.print("deserializeJson() failed: ");
      Serial.println(error.f_str());
      return;
    }

    String instruksi = doc["instruksi"];
    durasiServo = doc["durasiServo"].as<unsigned long>();
    durasiTurun = doc["durasiTurun"].as<unsigned long>();
    durasiRendam = doc["durasiRendam"].as<unsigned long>();

 // Simpan nilai baru ke Preferences
    preferences.putUInt("durasiServo", durasiServo);
    preferences.putUInt("durasiTurun", durasiTurun);
    preferences.putUInt("durasiRendam", durasiRendam);

    Serial.println("Durasi Servo diperbarui: " + String(durasiServo) + " detik");
    Serial.println("Durasi Turun diperbarui: " + String(durasiTurun) + " detik");
    Serial.println("Durasi Rendam diperbarui: " + String(durasiRendam) + " menit");


    if (instruksi == "otomatis") {
      if (!processActive) {
        processActive = true;
        servoActive = true;
        startProcess("Feeding", durasiServo);
        setFeedStart();
        myServo.write(90);
        Serial.println("Proses otomatis dimulai.");
      }
    }
  }
  // Penanganan untuk topik "kontrol" dengan perintah sederhana
  else if (String(topic) == mqttTopicKontrol) {
    if (message == "naik") {
      digitalWrite(relay1, HIGH);
      digitalWrite(relay2, LOW);
      clearKeterangan();
      lcd.setCursor(2, 0);
      lcd.print("Instruksi:");
      lcd.setCursor(2, 1);
      lcd.print("Anco Naik");
      setIconUp();
      Serial.println("Instruksi: Anco Naik");
    } 
    else if (message == "turun") {
      digitalWrite(relay1, LOW);
      digitalWrite(relay2, HIGH);
      clearKeterangan();
      lcd.setCursor(2, 0);
      lcd.print("Instruksi:");
      lcd.setCursor(2, 1);
      lcd.print("Anco Turun");
      setIconDown(); // Perbaiki dari setIconUp() ke setIconDown()
      Serial.println("Instruksi: Anco Turun");
    } 
    else if (message == "stop") {
      stopAll();
      clearKeterangan();
      lcd.setCursor(2, 0);
      lcd.print("Instruksi:");
      lcd.setCursor(2, 1);
      lcd.print("Motor Stop");
      setIconStop();
      Serial.println("Instruksi: Motor Stop");
    }
  }
}



void setFeedStart() {
  lcd.setCursor(0, 1); 
  lcd.write(byte(0)); 
}

void setFeedStop() {
  lcd.setCursor(0, 1); 
  lcd.write(byte(1));
}

void setIconUp() {
  lcd.setCursor(0, 1); 
  lcd.write(byte(2));
//  lcd.setCursor(2, 1);
//  lcd.print("Anco Naik");
}

void setIconDown() {
  lcd.setCursor(0, 1); 
  lcd.write(byte(3)); 
//  lcd.setCursor(2, 1);
//  lcd.print("Anco Turun");
}

void setIconStop() {
  lcd.setCursor(0, 1); 
  lcd.write(byte(4)); 
}

void setLimitUp() {
  lcd.setCursor(0, 1); 
  lcd.write(byte(5));   
}

void setLimitDown() {
  lcd.setCursor(0, 1); 
  lcd.write(byte(6));    
}

void setSignalOn() {
//  lcd.setCursor(0, 0); 
//  lcd.write(byte(7));  
  lcd.createChar(7, isignalOn);  // Kembalikan karakter asli ke slot 7
  lcd.setCursor(0, 0);
  lcd.write(byte(7));
}

void setSignalOff() {
//  lcd.setCursor(0, 0); 
//  lcd.print("x");  
  lcd.createChar(7, isignalOff);  // Kembalikan karakter asli ke slot 7
  lcd.setCursor(0, 0);
  lcd.write(byte(7));
}

// Fungsi untuk membersihkan area tertentu pada LCD
void clearLCDArea(byte row, byte startCol, byte endCol) {
  // Pastikan baris dan kolom berada dalam rentang yang valid
  if (row >= lcdRows) return;
  if (startCol >= lcdColumns) return;
  if (endCol >= lcdColumns) endCol = lcdColumns - 1;

  // Set kursor ke posisi awal
  lcd.setCursor(startCol, row);
  
  // Hitung jumlah spasi yang perlu ditulis
  byte numSpaces = endCol - startCol + 1;
  
  // Tulis spasi sebanyak yang diperlukan
  for (byte i = 0; i < numSpaces; i++) {
    lcd.print(" ");
  }
}

void clearKeterangan(){
 // Bersihkan kembali area tertentu
  clearLCDArea(0, 2, lcdColumns - 1); // Baris 0, kolom 2-15
  clearLCDArea(1, 2, lcdColumns - 1); // Baris 1, kolom 2-15
}

void checkConnections() {
  if (!remoteMode) {
    // Jika tidak dalam remoteMode, tidak perlu memeriksa koneksi
    return;
  }
  bool wifiConnected = (WiFi.status() == WL_CONNECTED);
  bool mqttConnected = client.connected();

  // Jika WiFi terputus, coba reconnect
  if (!wifiConnected) {
    Serial.println("WiFi disconnected. Attempting to reconnect...");
    setSignalOff(); // Update ikon LCD untuk menunjukkan status terputus
    wifi_connect(); // Coba reconnect WiFi
  }

  // Jika MQTT terputus, coba reconnect
  if (wifiConnected && !mqttConnected) {
    Serial.println("MQTT disconnected. Attempting to reconnect...");
    setSignalOff(); // Update ikon LCD untuk menunjukkan status terputus
    mqtt_connect(); // Coba reconnect MQTT
  }

  // Jika kedua koneksi terhubung, update ikon LCD
  if (wifiConnected && mqttConnected) {
    setSignalOn();
  } else {
    setSignalOff();
  }
}


void disconnectWiFi() {
  if (WiFi.status() == WL_CONNECTED) {
    WiFi.disconnect(true); // Memutuskan koneksi dan menghapus SSID/password
    Serial.println("WiFi disconnected.");
    // Perbarui LCD jika perlu
    clearKeterangan();
    lcd.setCursor(2, 0);
    lcd.print("Net status:");
    lcd.setCursor(2, 1);
    lcd.print("Disconnected");
    setSignalOff();
  }
}


// Fungsi untuk menampilkan informasi timer pada LCD
void displayInfoTimer() {
  clearKeterangan();
// Baris Pertama: "Motor Stop"
// clearLCDArea(0, 0, lcdColumns - 1); // Bersihkan seluruh baris pertama
  lcd.setCursor(2, 0);
  lcd.print("Motor Stop");

// Baris Kedua: "F: 5s T:5s R:1m"
// clearLCDArea(1, 0, lcdColumns - 1); // Bersihkan seluruh baris kedua
  lcd.setCursor(2, 1);
  lcd.print("F:" + String(durasiServo) + "s ");
  lcd.print("T:" + String(durasiTurun) + "s ");
  lcd.print("R:" + String(durasiRendam) + "m");
}