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

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 = "555555";

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

// Function prototypes
int readSensor(int trigPin, int echoPin);
void activateRGBLED(int redPin, int greenPin, int bluePin, int redValue, int greenValue, int blueValue);
void deactivateRGBLED(int redPin, int greenPin, int bluePin);
void unlock();

void setup() {
  Serial.begin(9600);
  servo1.attach(A15);  // Sesuaikan pin servo1
  servo2.attach(A14); // Sesuaikan pin servo2
  servo3.attach(A13); // Sesuaikan pin servo3

  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);

  // Set konfigurasi lampu pada setup awal
  activateRGBLED(redPin1, greenPin1, bluePin1, 0, 0, 0);
  activateRGBLED(redPin2, greenPin2, bluePin2, 0, 0, 0);
  activateRGBLED(redPin3, greenPin3, bluePin3, 0, 0, 0);
}

void loop() {
  unlock();  // Panggil fungsi untuk membuka kunci (putar servo)
}

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

  if (distance1 > 2) {
    // Memutar servo 90 derajat untuk slot 1
    servo1.write(90);
    delay(5000); // Delay untuk memberikan waktu servo bergerak
    // Mematikan servo untuk slot 1
    servo1.write(0);
    // Set status slot baterai 1 menjadi terisi
    batterySlotStatus1 = true;
  } else {
    // Set status slot baterai 1 menjadi kosong
    batterySlotStatus1 = false;
  }

  if (distance2 > 2) {
    // Memutar servo 90 derajat untuk slot 2
    servo2.write(90);
    delay(5000); // Delay untuk memberikan waktu servo bergerak
    // Mematikan servo untuk slot 2
    servo2.write(0);
    // Set status slot baterai 2 menjadi terisi
    batterySlotStatus2 = true;
  } else {
    // Set status slot baterai 2 menjadi kosong
    batterySlotStatus2 = false;
  }

  if (distance3 > 2) {
    // Memutar servo 90 derajat untuk slot 3
    servo3.write(90);
    delay(5000); // Delay untuk memberikan waktu servo bergerak
    // Mematikan servo untuk slot 3
    servo3.write(0);
    // Set status slot baterai 3 menjadi terisi
    batterySlotStatus3 = true;
  } else {
    // Set status slot baterai 3 menjadi kosong
    batterySlotStatus3 = false;
  }

  // Aktifkan dan nonaktifkan warna lampu sesuai dengan status baterai
  if (batterySlotStatus1) {
    activateRGBLED(redPin1, greenPin1, bluePin1, 255, 0, 0);  // Merah
  } else {
    deactivateRGBLED(redPin1, greenPin1, bluePin1);
  }

  if (batterySlotStatus2) {
    activateRGBLED(redPin2, greenPin2, bluePin2, 255, 0, 0);  // Hijau
  } else {
    deactivateRGBLED(redPin2, greenPin2, bluePin2);
  }

  if (batterySlotStatus3) {
    activateRGBLED(redPin3, greenPin3, bluePin3, 255, 0, 0);  // Biru
  } else {
    deactivateRGBLED(redPin3, greenPin3, bluePin3);
  }

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

void activateRGBLED(int redPin, int greenPin, int bluePin, int redValue, int greenValue, int blueValue) {
  // Activate RGB LED with specified color values
  analogWrite(redPin, redValue);
  analogWrite(greenPin, greenValue);
  analogWrite(bluePin, blueValue);
}

void deactivateRGBLED(int redPin, int greenPin, int bluePin) {
  // Deactivate RGB LED
  analogWrite(redPin, 0);
  analogWrite(greenPin, 0);
  analogWrite(bluePin, 0);
}

int readSensor(int trigPin, int echoPin) {
  // Your existing code for reading the distance sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  return pulseIn(echoPin, HIGH) / 30; // Convert time to distance (cm)
}