//==================================================
//== INCLUDES DAN OBJEK
//==================================================
#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

// Layar LCD 16*2 
// PERINGATAN: Alamat 0x27 mungkin salah. 
// Gunakan "I2C Scanner" untuk menemukan alamat yang benar.
LiquidCrystal_I2C lcd(0x27, 16, 2);

//==================================================
//== KONFIGURASI PIN DAN SISTEM
//==================================================
//Konfig PIN
#define TRIG_ALL 2
#define ECHO_IN 3
#define ECHO_OUT 4
int ECHO_SLOT[6] = {5, 6, 7, 8, 12, 13};

#define SERVO_IN_PIN 9
#define SERVO_OUT_PIN 10
#define LEDD_FULL 11 // Sesuai kode Anda

//OBJ SERVO
Servo servoIn;
Servo servoOut;

//KONSTANTA SISTEM
#define TOTAL_SLOTS 6     // Sesuai kode Anda
#define DIST_GATE 30      // JARAK MOBIL TERDETEKSI DI GERBANG DALAM CM
#define DIST_PASS 60      // JARAK MOBIL DIANGGAP SUDAH PERGI
#define DIST_SLOTS 15     // JARAK MOBIL TERDETEKSI PADA PARKIRAN
unsigned long gateDelay = 200;
unsigned long lastDisplay = 0;

//VARIABEL STATUS
bool gateInOpen = false;
bool gateOutOpen = false;
int slotOccupied = 0;

//==================================================
//== FUNGSI BACA SENSOR (DARI KODE ANDA)
//==================================================
// <-- PERBAIKAN 1: pinMode() dihapus dari sini
long readDistanceCM(int echoPin) {
  // pinMode(TRIG_ALL, OUTPUT); <-- DIHAPUS, sudah ada di setup()
  
  digitalWrite(TRIG_ALL, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_ALL, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_ALL, LOW);
  
  // pinMode(echoPin, INPUT); <-- DIHAPUS, sudah ada di setup()
  
  long duration = pulseIn(echoPin, HIGH, 25000);
  long distance = duration * 0.034 / 2;
  if (distance == 0) distance = 999;
  return distance;
}

//==================================================
//== FUNGSI SETUP (DARI GAMBAR ANDA)
//==================================================
void setup() {
  // Inisialisasi Pin Sensor Ultrasonik
  pinMode(TRIG_ALL, OUTPUT);
  pinMode(ECHO_IN, INPUT);
  pinMode(ECHO_OUT, INPUT);

  // Inisialisasi Pin Echo untuk 6 Slot Parkir
  for (int i = 0; i < 6; i++) {
    pinMode(ECHO_SLOT[i], INPUT);
  }

  // Inisialisasi Pin LED Penuh
  pinMode(LEDD_FULL, OUTPUT); 

  // Inisialisasi Servo
  servoIn.attach(SERVO_IN_PIN);
  servoOut.attach(SERVO_OUT_PIN);
  servoIn.write(0);  // Pastikan servo tertutup saat mulai
  servoOut.write(0); // Pastikan servo tertutup saat mulai

  // Inisialisasi LCD 16x2
  lcd.init();      
  lcd.backlight(); 
  lcd.clear();     

  // Menampilkan pesan Inisialisasi di LCD
  lcd.setCursor(0, 0); // Baris pertama
  lcd.print("Smart Parking"); 
  lcd.setCursor(0, 1); // Baris kedua
  lcd.print("Init...");
  
  delay(1500);
}

//==================================================
//== FUNGSI TAMPILAN LCD (LOGIKA DARI OLED ANDA)
//==================================================
void drawDisplay() {
  // Bersihkan layar LCD (Catatan: clear() bisa bikin kedip,
  // tapi kita ikuti logika awal dulu)
  lcd.clear(); 

  int availableSlots = TOTAL_SLOTS - slotOccupied;

  // --- Baris 0 (Baris Pertama) ---
  lcd.setCursor(0, 0);
  lcd.print("Terisi: ");
  lcd.print(slotOccupied);
  lcd.print(" / ");
  lcd.print(TOTAL_SLOTS);

  // --- Baris 1 (Baris Kedua) ---
  if (availableSlots == 0) {
    lcd.setCursor(0, 1);
    lcd.print("STATUS: PENUH");
    digitalWrite(LEDD_FULL, HIGH);
  } 
  else {
    lcd.setCursor(0, 1);
    lcd.print("STATUS: TERSEDIA");
    digitalWrite(LEDD_FULL, LOW);
  }
}

//==================================================
//== FUNGSI LOOP UTAMA (GABUNGAN DARI 3 GAMBAR)
//==================================================
void loop() {
  // --- Baca sensor gate ---
  long distIn = readDistanceCM(ECHO_IN);
  delay(50); // <-- PERBAIKAN 2: Delay untuk mencegah crosstalk

  long distOut = readDistanceCM(ECHO_OUT);
  delay(50); // <-- PERBAIKAN 2: Delay untuk mencegah crosstalk

  // --- Hitung slot terisi ---
  int count = 0;
  for (int i = 0; i < 6; i++) {
    long d = readDistanceCM(ECHO_SLOT[i]);
    if (d < DIST_SLOT) count++;
    delay(50); // <-- PERBAIKAN 2: Delay untuk mencegah crosstalk
  }
  slotOccupied = count;

  // Variabel lokal untuk logika gate
  int availableSlots = TOTAL_SLOTS - slotOccupied;

  // --- Mobil Masuk --- 
  if (distIn < DIST_GATE && availableSlots > 0 && !gateInOpen) {
    servoIn.write(90);
    gateInOpen = true;
  }
  if (gateInOpen && distIn > DIST_PASS) {
    servoIn.write(0);
    gateInOpen = false;
  }

  // --- Mobil Keluar ---
  if (distOut < DIST_GATE && !gateOutOpen) {
    servoOut.write(90);
    gateOutOpen = true;
  }
  if (gateOutOpen && distOut > DIST_PASS) {
    servoOut.write(0);
    gateOutOpen = false;
  }

  // --- Update LCD tiap 500 ms --- 
  if (millis() - lastDisplay > 500) {
    drawDisplay(); 
    lastDisplay = millis();
  }

  delay(100); 
}