#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;
unsigned long durasiTurun = 2;
unsigned long durasiRendam = 2;

// Variabel status proses
unsigned long startTime = 0;
bool servoActive = false;
bool turunActive = false;
bool rendamActive = false;
bool processActive = false;
bool checkLimitSwitch = false;  // Untuk pengecekan limit switch

// Karakter khusus untuk LCD
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 isignal[] = {B00001, B00011, B00111, B01111, B11111, B11111, B00000, B00000};


// Deklarasi fungsi-fungsi
// void updateTimeLeft();  
// void stopAll();  
// void stopBecauseLimit();  
// void startProcess(const char *status, unsigned long duration);  
// void displayStatus(const char *status);  
// bool checkTimeElapsed(unsigned long duration);  
// void clearBaris1();  

// Fungsi kontrol tampilan di LCD
void setFeedStart();  
void setFeedStop();  
void setUp();  
void setDown();  
void setStop();  
void setLimitUp();  
void setLimitDown();  
void setSignalOn();  
void setSignalOff();


void setup() {
  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);

  // Membuat karakter khusus di LCD
  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, isignal);

  lcd.setCursor(0, 0);
  lcd.print("Ready");
  delay(1000);
  lcd.clear();
}

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

  if (btnEmergencyStop.isPressed()) {
    stopAll();
  }

  if (checkLimitSwitch && digitalRead(limitSwitchPin) == LOW) {
    stopBecauseLimit();
  }

  if (buttonServo.isPressed() && !processActive) {
    if (digitalRead(limitSwitchPin) == HIGH) {
      displayStatus("Atur Posisi Anco!");
    } else {
      processActive = true;
      startProcess("Servo Buka", durasiServo);
      myServo.write(90);
      servoActive = true;
      setFeedStart();
    }
  }

  if (servoActive && checkTimeElapsed(durasiServo)) {
    myServo.write(0);
    displayStatus("Servo Tutup");
    servoActive = false;
    turunActive = true;  // Lanjut ke proses turun
    startProcess("Turun", durasiTurun);
    setFeedStop();
  }

  if (turunActive && checkTimeElapsed(durasiTurun)) {
    turunActive = false;
    rendamActive = true;  // Lanjut ke proses rendam
    startProcess("Rendam", durasiRendam);
  }

  if (rendamActive && checkTimeElapsed(durasiRendam)) {
    rendamActive = false;
    processActive = false;  // Semua proses selesai
   // lcd.clear();
   // lcd.print("Proses Selesai");
    displayStatus("Proses Selesai");
  }

  if (buttonCW.isPressed()) {
    setUp();
    digitalWrite(relay1, HIGH);
    digitalWrite(relay2, LOW);
  }

  if (buttonCCW.isPressed()) {
    setDown();
    digitalWrite(relay1, LOW);
    digitalWrite(relay2, HIGH);
  }

  updateTimeLeft();  // Perbarui waktu tersisa di LCD
  
}

// Fungsi untuk memperbarui waktu tersisa di baris kedua LCD
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 - elapsedTime);
    }

    lcd.setCursor(0, 1);
    lcd.print("Time Left: ");
    lcd.print(timeLeft);
    lcd.print("s  ");
  } else {
    lcd.setCursor(0, 1);
    lcd.print("                ");
  }
}

// Fungsi untuk menghentikan semua proses
void stopAll() {
  digitalWrite(relay1, LOW);
  digitalWrite(relay2, LOW);
  setStop();
  processActive = false;
  clearBaris1();
}

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

void startProcess(const char *status, unsigned long duration) {
  startTime = millis();
  displayStatus(status);
}

void displayStatus(const char *status) {
 // lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print(status);
  Serial.println(status);
}

bool checkTimeElapsed(unsigned long duration) {
  return millis() - startTime >= duration * 1000;
}

void clearBaris1() {
 // lcd.setCursor(0, 1);
 // lcd.print("                ");
}

// Fungsi untuk kontrol tampilan LCD
void setFeedStart() {
  lcd.setCursor(0, 0);
  lcd.write(byte(0));  // Tampilkan karakter feedStart
}

void setFeedStop() {
  lcd.setCursor(0, 0);
  lcd.write(byte(1));  // Tampilkan karakter feedStop
}

void setUp() {
  lcd.setCursor(2, 0);
  lcd.write(byte(2));  // Tampilkan karakter UP
}

void setDown() {
  lcd.setCursor(2, 0);
  lcd.write(byte(3));  // Tampilkan karakter DOWN
}

void setStop() {
  lcd.setCursor(2, 0);
  lcd.write(byte(4));  // Tampilkan karakter STOP
}

void setLimitUp() {
  lcd.setCursor(4, 0);
  lcd.write(byte(5));  // Tampilkan karakter limit UP
}

void setLimitDown() {
  lcd.setCursor(4, 0);
  lcd.write(byte(6));  // Tampilkan karakter limit DOWN
}

void setSignalOn() {
  lcd.setCursor(6, 0);
  lcd.write(byte(7));  // Tampilkan karakter sinyal ON
}

void setSignalOff() {
  lcd.setCursor(6, 0);
  lcd.print("x");  // Tampilkan 'x' untuk sinyal OFF
}