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

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] = {5, 4, 3, 2};
byte colPins[COLS] = {A3, A2, A1, A0};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);
LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo servo1;
Servo servo2;
Servo servo3;

const int trigPin1 = 23;
const int echoPin1 = 25;
const int redPin1 = 22;
const int greenPin1 = 24;
const int bluePin1 = 26;

const int trigPin2 = 27;
const int echoPin2 = 29;
const int redPin2 = 33;
const int greenPin2 = 35;
const int bluePin2 = 37;

const int trigPin3 = 41;
const int echoPin3 = 43;
const int redPin3 = 47;
const int greenPin3 = 49;
const int bluePin3 = 51;

// Set password 6 digit yang diinginkan
String password = "123456";

// Tambahkan variabel global untuk menyimpan waktu deteksi objek
unsigned long lastDetectionTime1 = 0;
unsigned long lastDetectionTime2 = 0;
unsigned long lastDetectionTime3 = 0;

// Variabel status slot baterai
bool batterySlotStatus1 = false;
bool batterySlotStatus2 = false;
bool batterySlotStatus3 = false;

// Variabel untuk menyimpan status apakah servo sudah ditutup atau belum
bool isServoClosed = false;

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);
  lcd.backlight();
  servo1.attach(A15);  
  servo2.attach(A14); 
  servo3.attach(A13); 

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(redPin1, OUTPUT);
  pinMode(greenPin1, OUTPUT);
  pinMode(bluePin1, OUTPUT);

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(redPin2, OUTPUT);
  pinMode(greenPin2, OUTPUT);
  pinMode(bluePin2, OUTPUT);

  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(redPin3, OUTPUT);
  pinMode(greenPin3, OUTPUT);
  pinMode(bluePin3, OUTPUT);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Welcome");
  delay(2000);
}

void loop() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Masukan Kode");

  String inputCode = "";  // Inisialisasi string inputCode
  unsigned long startTime = millis(); // Waktu awal input

  while (inputCode.length() < 6 && millis() - startTime < 50000) {
    char key = keypad.getKey();
    if (key) {
      inputCode += key;
      lcd.setCursor(inputCode.length() - 1, 1);
      lcd.print('*');
    }

    // Cek apakah tombol '#' ditekan
    if (key == '#') {
      break;
    }

    delay(100);
  }

  // Cek apakah password sesuai
  if (inputCode.length() == 6 && inputCode == password) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Berhasil!");

    unlock();  // Panggil fungsi untuk membuka kunci (putar servo)
    delay(2000);
  } else {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Gagal! Password salah");
    lcd.setCursor(0, 1);
    lcd.print("Coba lagi");
    delay(2000);
  }
  // Tambahkan logika atau proses selanjutnya di sini setelah kunci terbuka
}

void unlock() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Unlocking...");

  int distance1 = readSensor(trigPin1, echoPin1);
  int distance2 = readSensor(trigPin2, echoPin2);
  int distance3 = readSensor(trigPin3, echoPin3);

  if (distance1 > 10) {
    activateLEDandServo(redPin1, greenPin1, bluePin1, servo1);
  } else {
    deactivateLEDandServo(redPin1, greenPin1, bluePin1, servo1);
  }

  if (distance2 > 10) {
    activateLEDandServo(redPin2, greenPin2, bluePin2, servo2);
  } else {
    deactivateLEDandServo(redPin2, greenPin2, bluePin2, servo2);
  }

  if (distance3 > 10) {
    activateLEDandServo(redPin3, greenPin3, bluePin3, servo3);
  } else {
    deactivateLEDandServo(redPin3, greenPin3, bluePin3, servo3);
  }

  delay(100); // Delay untuk stabilitas pembacaan sensor
}

int readSensor(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  return pulseIn(echoPin, HIGH) / 30; // Konversi waktu menjadi jarak (cm)
}

void activateLEDandServo(int redPin, int greenPin, int bluePin, Servo servo) {
  Serial.println("Activate servo...");
  analogWrite(redPin, 255);
  analogWrite(greenPin, 0);
  analogWrite(bluePin, 0);
  servo.writeMicroseconds(1500); // Nilai tengah (90 derajat)
  Serial.println("Servo activated.");
}

void deactivateLEDandServo(int redPin, int greenPin, int bluePin, Servo servo) {
  Serial.println("Deactivate servo...");
  analogWrite(redPin, 0);
  analogWrite(greenPin, 0);
  analogWrite(bluePin, 255);
  servo.writeMicroseconds(1500); // Nilai tengah (90 derajat)
  Serial.println("Servo deactivated.");
}