#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <ezButton.h>  // Tambahkan library ezButton

// Inisialisasi LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Inisialisasi Servo
Servo myServo;
int servoPin = 4;  // Servo di GPIO 4

// Pin relay dan limit switch
const int relay1 = 32;
const int relay2 = 33;
const int limitSwitchPin = 5;  // Limit Switch

// Objek ezButton untuk setiap tombol
ezButton buttonCW(12);       // Tombol CW (UP)
ezButton buttonCCW(13);      // Tombol CCW (DOWN)
ezButton buttonServo(27);    // Tombol Servo
ezButton btnEmergencyStop(14);  // Tombol Emergency Stop

// Variabel durasi
unsigned long durasiServo = 2;  // Durasi servo dalam detik
unsigned long durasiTurun = 2;  // Durasi turun
unsigned long durasiRendam =2;  // Durasi rendam

// Waktu dan status proses
unsigned long startTime = 0;
bool servoActive = false;
bool turunActive = false;
bool rendamActive = false;
bool upActive = false;
bool processActive = false;  // Indikator apakah proses sedang berjalan
bool checkLimitSwitch = false;  // Indikator pengecekan limit switch

void setup() {
  // Inisialisasi Serial Monitor dan LCD
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();

  // Inisialisasi tombol dan limit switch
  buttonCW.setDebounceTime(50);
  buttonCCW.setDebounceTime(50);
  buttonServo.setDebounceTime(50);
  btnEmergencyStop.setDebounceTime(50);

  pinMode(limitSwitchPin, INPUT_PULLUP);
  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);

  // Relay awal OFF
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);

  // Inisialisasi Servo
  myServo.attach(servoPin, 500, 2400);
  myServo.write(0);  // Servo mulai dari posisi 0 derajat
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Ready");
  delay(1000);
}

void loop() {
  // Update status tombol
  buttonCW.loop();
  buttonCCW.loop();
  buttonServo.loop();
  btnEmergencyStop.loop();

  // Cek Emergency Stop
  if (btnEmergencyStop.isPressed()) {
    stopAll();
    return;
  }

  // Pengecekan Limit Switch hanya jika dalam mode UP
  if (checkLimitSwitch && digitalRead(limitSwitchPin) == LOW) {
    stopBecauseLimit();
    return;
  }

  // Tombol Servo: Memulai proses baru
  if (buttonServo.isPressed() && !processActive) {
    if (digitalRead(limitSwitchPin) == HIGH) {
      // Jika limit switch tidak aktif, tampilkan pesan dan kembali ke loop
      displayStatus("Atur Posisi Anco!");
    } else {
      // Jika limit switch aktif, mulai proses servo
      processActive = true;
      startProcess("Servo Buka", durasiServo);
      myServo.write(90);  // Servo buka 90 derajat
      servoActive = true;
    }
  }

  // Proses Servo selesai, lanjut ke Turun
  if (servoActive && checkTimeElapsed(durasiServo)) {
    myServo.write(0);  // Servo tutup
    displayStatus("Servo Tutup");
    servoActive = false;
    startProcess("Turun", durasiTurun);
    turunActive = true;
    digitalWrite(relay1, LOW);
    digitalWrite(relay2, HIGH);  // Perintah Turun
  }

  // Proses Turun selesai, lanjut ke Rendam
  if (turunActive && checkTimeElapsed(durasiTurun)) {
    turunActive = false;
    startProcess("Rendam", durasiRendam);
    rendamActive = true;
    digitalWrite(relay1, LOW);
    digitalWrite(relay2, LOW);
  }

  // Proses Rendam selesai, lanjut ke UP
  if (rendamActive && checkTimeElapsed(durasiRendam)) {
    rendamActive = false;
    upActive = true;
    checkLimitSwitch = true;  // Aktifkan pengecekan limit switch saat UP
    displayStatus("UP");
    digitalWrite(relay1, HIGH);
    digitalWrite(relay2, LOW);

    // if (digitalRead(limitSwitchPin) == LOW) {
    //   stopAll();
    //   return;
    // }

    // resetProcess();
  }

  // Tombol CW (UP) dengan pengecekan Limit Switch
  if (buttonCW.isPressed()) {
    checkLimitSwitch = true;  // Aktifkan pengecekan limit switch
    displayStatus("UP");
    digitalWrite(relay1, HIGH);
    digitalWrite(relay2, LOW);
  }

  // Tombol CCW (DOWN)
  if (buttonCCW.isPressed()) {
    checkLimitSwitch = false;  // Nonaktifkan pengecekan limit switch
    displayStatus("DOWN");
    digitalWrite(relay1, LOW);
    digitalWrite(relay2, HIGH);
  }

  delay(100);  // Jeda agar mudah dibaca
}

// Fungsi untuk menghentikan semua proses
void stopAll() {
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  displayStatus("STOP");
  resetProcess();
}

void stopBecauseLimit() {
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  displayStatus("LIMIT ACTIVED");
  resetProcess();
}

// Fungsi untuk memulai proses dan menampilkan status di LCD
void startProcess(const char *status, unsigned long duration) {
  startTime = millis();
  displayStatus(status);
}

// Fungsi untuk menampilkan status di baris 0 LCD
void displayStatus(const char *status) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(status);
  Serial.println(status);
}

// Fungsi untuk mengecek apakah waktu proses sudah habis
bool checkTimeElapsed(unsigned long duration) {
  unsigned long elapsedTime = (millis() - startTime) / 1000;
  unsigned long timeLeft = duration > elapsedTime ? duration - elapsedTime : 0;

  // Tampilkan waktu tersisa di baris 1 LCD
  lcd.setCursor(0, 1);
  lcd.print("Time Left: ");
  lcd.print(timeLeft);

  return timeLeft == 0;
}

// Fungsi untuk mereset status proses
void resetProcess() {
  servoActive = false;
  turunActive = false;
  rendamActive = false;
  upActive = false;
  processActive = false;
  checkLimitSwitch = false;  // Nonaktifkan pengecekan limit switch
}