#include <Keypad.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

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

// Keypad Setup
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
byte rowPins[ROWS] = {9, 8, 7, 6};
byte colPins[COLS] = {5, 4, 3, 2};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Passwords
String inputPassword = "";
String passwordServo1 = "1234";
String passwordServo2 = "4321";

// Servo setup
Servo servo1;
Servo servo2;

const int green1 = 12;
const int green2 = 13;

void resetInput() {
  inputPassword = "";
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Enter Password:");
  lcd.setCursor(0, 1);
}

void setup() {
  lcd.begin(16, 2);
  lcd.backlight();
  resetInput();

  pinMode(green1, OUTPUT);
  pinMode(green2, OUTPUT);

  // Attach servo to pins
  servo1.attach(10); // Servo 1 ke pin 10
  servo2.attach(11); // Servo 2 ke pin 11

  // Awal posisi servo
  servo1.write(0);
  servo2.write(0);
}

void loop() {
  char key = keypad.getKey();

  if (key) {
    if (key == '#') {
      if (inputPassword == passwordServo1) {
        lcd.clear();
        lcd.print("Servo 1 Aktif");
        servo1.write(90); // gerak servo 1
        delay(2000);
        servo1.write(0);
        digitalWrite(green1, HIGH);
        delay(2000);
        digitalWrite(green1, LOW);
      }
      else if (inputPassword == passwordServo2) {
        lcd.clear();
        lcd.print("Servo 2 Aktif");
        servo2.write(90); // gerak servo 2
        delay(2000);
        servo2.write(0);
        digitalWrite(green2, HIGH);
        delay(2000);
        digitalWrite(green2, LOW);
      }
      else {
        lcd.clear();
        lcd.print("Wrong Password");
    
        
      }

      delay(2000);
      resetInput();
    }
    else if (key == '*') {
      resetInput();
    }
    else {
      inputPassword += key;
      lcd.setCursor(0, 1);
      lcd.print(inputPassword);

      // Untuk masking input (opsional):
      // lcd.print(String(inputPassword.length(), '*'));
    }
  }
}