// #include <LiquidCrystal.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);


// tidak menggunakan rfid karena komponentnya disini belum tersedia

// Bergerak 180 derajat dikarenakan belum support dengan 360
Servo servoMotor1;
Servo servoMotor2;

// Pin push button
const int buttonBiru = 2; 
const int buttonMerah = 4;

int button1State = HIGH;
int button2State = HIGH;
int lastButton1State = HIGH;
int lastButton2State = HIGH;

// Variabel untuk debounce
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50; // Waktu debounce dalam milidetik

void setup() {
  Serial.begin(9600);

  // Inisialisasi LCD
  lcd.begin(16, 2);
  lcd.print("Silahkan pilih");
  lcd.setCursor(0, 1);
  lcd.print("tombol...");

  pinMode(buttonBiru, INPUT_PULLUP);
  pinMode(buttonMerah, INPUT_PULLUP);

  // Inisialisasi servo
  servoMotor1.attach(3);
  servoMotor2.attach(5); 
  servoMotor1.write(0);  
  servoMotor2.write(0);  
}

void loop() {
  int reading1 = digitalRead(buttonBiru);
  int reading2 = digitalRead(buttonMerah);

  // Jika tombol pertama telah berubah status
  if (reading1 != lastButton1State) {
    lastDebounceTime = millis(); // Update waktu debounce
  }

  // Jika tombol pertama sudah stabil selama debounceDelay
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading1 != button1State) {
      button1State = reading1;
      if (button1State == LOW) { // Tombol pertama ditekan (LOW berarti aktif)
        // Hapus layar LCD
        lcd.clear();
        lcd.print("Servo 1 Berputar!");

        for (int pos = 0; pos <= 180; pos++) {
          servoMotor1.write(pos); 
          delay(5);
        }

        for (int pos = 180; pos >= 0; pos--) {
          servoMotor1.write(pos);
          delay(5); 
        }

        lcd.clear();
        lcd.print("Selesai!");

        // Kembali ke pesan awal
        lcd.clear();
        lcd.print("Silahkan pilih");
        lcd.setCursor(0, 1);
        lcd.print("tombol...");
      }
    }
  }

  // Jika tombol kedua telah berubah status
  if (reading2 != lastButton2State) {
    lastDebounceTime = millis(); // Update waktu debounce
  }

  // Jika tombol kedua sudah stabil selama debounceDelay
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading2 != button2State) {
      button2State = reading2;
      if (button2State == LOW) { 
        lcd.clear();
        lcd.print("Servo 2 Berputar!");

        for (int pos = 0; pos <= 180; pos++) {
          servoMotor2.write(pos); 
          delay(5); 
        }

        for (int pos = 180; pos >= 0; pos--) {
          servoMotor2.write(pos); // Servo kedua kembali
          delay(5); 
        }

        lcd.clear();
        lcd.print("Selesai!");
        delay(2000);

        lcd.clear();
        lcd.print("Silahkan pilih");
        lcd.setCursor(0, 1);
        lcd.print("tombol...");
      }
    }
  }

  // Simpan status tombol terakhir
  lastButton1State = reading1;
  lastButton2State = reading2;
}