#include <ESP32Servo.h>           // Library untuk kontrol servo
#include <Wire.h>                 // Library I2C
#include <LiquidCrystal_I2C.h>    // Library untuk LCD

// Inisialisasi LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // Alamat I2C 0x27, ukuran LCD 16x2

// Inisialisasi Servo
Servo servoMasuk;  // Servo untuk palang masuk
Servo servoKeluar; // Servo untuk palang keluar

// Pin untuk sensor jarak
const int trigPin1 = 13; // Sensor jarak untuk kendaraan masuk
const int echoPin1 = 12;
const int trigPin2 = 2;  // Sensor jarak untuk kendaraan keluar
const int echoPin2 = 42;

// Variabel
const int maxSlotCar = 4;   // Batas maksimum slot parkir
int Slot = 4; // Total slot parkir
int flag1 = 0;
int flag2 = 0;

// Fungsi untuk membaca jarak dari sensor
long readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2; // Konversi ke cm
  return distance;
}

void setup() {
  // Inisialisasi Serial Monitor
  Serial.begin(115200);

  // Inisialisasi pin sensor
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  // Inisialisasi servo
  servoMasuk.attach(14);  // Servo untuk palang masuk
  servoKeluar.attach(1); // Servo untuk palang keluar

  // Posisi awal palang (tertutup)
  servoMasuk.write(100);
  servoKeluar.write(100);

  // Inisialisasi LCD
  Wire.begin(10, 8);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Parking System");
  lcd.setCursor(0, 1);
  lcd.print("Initializing...");
  delay(2000);
  lcd.clear();

  // Tampilkan slot awal
  lcd.setCursor(0, 0);
  lcd.print("Slot Mobil : ");
  lcd.print(Slot);
  Serial.println("ESP32-S3 Parking System Initialized");
  Serial.println("Total Slots Available: " + String(Slot));
  Serial.println("-----------------------------------");
}

void loop() {
  // Membaca jarak dari sensor masuk dan keluar
  long distance1 = readDistance(trigPin1, echoPin1); // Sensor masuk
  long distance2 = readDistance(trigPin2, echoPin2); // Sensor keluar

  // Kendaraan masuk
  if (distance1 < 20 && flag1 == 0) { // Jika jarak kurang dari 20 cm
    if (Slot > 0) {
      flag1 = 1;
      servoMasuk.write(0); // Buka palang masuk
      Slot -= 1;           // Kurangi slot
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Kendaraan Masuk");
      lcd.setCursor(0, 1);
      lcd.print("Slot Mobil : ");
      lcd.print(Slot);
      Serial.println("Kendaraan Masuk. Slot Tersisa: " + String(Slot));
    } else {
      // Tidak ada slot tersedia
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Maaf");
      lcd.setCursor(0, 1);
      lcd.print("Parkiran Penuh");
      Serial.println("Maaf, parkiran penuh.");
    }
  }

  // Kendaraan keluar
  if (distance2 < 20 && flag2 == 0) { // Jika jarak kurang dari 20 cm
   if (Slot < maxSlotCar) { // Periksa apakah Slot tidak melebihi kapasitas maksimum
        flag2 = 1;
        servoKeluar.write(0); // Buka palang keluar
        Slot += 1;            // Tambah slot
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Kendaraan Keluar");
        lcd.setCursor(0, 1);
        lcd.print("Slot Mobil : ");
        lcd.print(Slot);
        Serial.println("Kendaraan Keluar. Slot Tersisa: " + String(Slot));
    } else {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Error, tidak ada ");
        lcd.setCursor(0, 1);
        lcd.print("yang parkir!");
        Serial.println("Error : Tidak ada yang parkir!.");
    }
  }

  // Palang menutup setelah kendaraan masuk/keluar
  if (flag1 == 1 && distance1 > 20) {
    delay(1000);
    servoMasuk.write(100);  // Tutup palang masuk
    servoKeluar.write(100); // Tutup palang keluar
    flag1 = 0;
    Serial.println("Palang Masuk Ditutup.");
  }

  // Palang menutup setelah kendaraan masuk/keluar
  if (flag2 == 1 && distance2 > 20) {
    delay(1000);

    servoMasuk.write(100);  // Tutup palang masuk
    servoKeluar.write(100); // Tutup palang keluar
    flag2 = 0;
    Serial.println("Palang Keluar Ditutup.");
  }

  // Tampilkan status sistem di Serial Monitor dan LCD
  delay(1000); // Memberikan jeda agar pembacaan stabil
  lcd.setCursor(0, 0);
  Serial.println("Status Sistem: Slot Tersisa " + String(Slot));
}