#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

// --- KONFIGURASI PIN ---
#define TRIG_PIN_MASUK 9
#define ECHO_PIN_MASUK 10
#define TRIG_PIN_KELUAR 5
#define ECHO_PIN_KELUAR 6

#define SERVO_PIN_MASUK 3
#define SERVO_PIN_KELUAR 4

#define LED_HIJAU_MASUK 12 
#define LED_MERAH_MASUK 11 
#define LED_HIJAU_KELUAR 8 
#define LED_MERAH_KELUAR 7 

// --- KONFIGURASI OLED ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1 

// Alamat 0x3C adalah yang paling umum
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// --- KONSTANTA SISTEM ---
const long JARAK_DETEKSI_CM = 15; 
const int POSISI_TUTUP = 0;   
const int POSISI_BUKA = 90;   
const long DELAY_LEWAT_MS = 3000; 

// --- VARIABEL GLOBAL ---
Servo servoMasuk;
Servo servoKeluar;
int jumlahKendaraan = 0;
bool isPalangMasukOpen = false; 
bool isPalangKeluarOpen = false;
unsigned long lastOpenTimeMasuk = 0;
unsigned long lastOpenTimeKeluar = 0;

// ===================================================
// FUNGSI UTILITY
// ===================================================

long measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 25000L); 
  if (duration == 0) return 999; 
  return duration * 0.034 / 2;
}

void updateOLED() {
  display.clearDisplay();
  
  display.setTextSize(1); 
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("SISTEM PARKIR ARDUINO");
  
  display.setTextSize(2); 
  display.setCursor(0, 30);
  display.print("Mobil: ");
  display.println(jumlahKendaraan);
  
  display.setTextSize(1); 
  display.setCursor(0, 55);
  display.print("MASUK: ");
  display.print(isPalangMasukOpen ? "BUKA" : "TUTUP");
  display.print(" KELUAR: ");
  display.print(isPalangKeluarOpen ? "BUKA" : "TUTUP");
  
  display.display(); 
}

void setPalangMasuk(bool open) {
  // ... (Fungsi tetap sama)
  if (open) {
    servoMasuk.write(POSISI_BUKA);
    digitalWrite(LED_HIJAU_MASUK, HIGH);
    digitalWrite(LED_MERAH_MASUK, LOW);
    isPalangMasukOpen = true;
    lastOpenTimeMasuk = millis();
  } else {
    servoMasuk.write(POSISI_TUTUP);
    digitalWrite(LED_HIJAU_MASUK, LOW);
    digitalWrite(LED_MERAH_MASUK, HIGH);
    isPalangMasukOpen = false;
  }
  Serial.print("Palang Masuk Status: "); Serial.println(open ? "Buka" : "Tutup");
  updateOLED();
}

void setPalangKeluar(bool open) {
  // ... (Fungsi tetap sama)
  if (open) {
    servoKeluar.write(POSISI_BUKA);
    digitalWrite(LED_HIJAU_KELUAR, HIGH);
    digitalWrite(LED_MERAH_KELUAR, LOW);
    isPalangKeluarOpen = true;
    lastOpenTimeKeluar = millis();
  } else {
    servoKeluar.write(POSISI_TUTUP);
    digitalWrite(LED_HIJAU_KELUAR, LOW);
    digitalWrite(LED_MERAH_KELUAR, HIGH);
    isPalangKeluarOpen = false;
  }
  Serial.print("Palang Keluar Status: "); Serial.println(open ? "Buka" : "Tutup");
  updateOLED(); 
}

// ===================================================
// SETUP
// ===================================================
void setup() {
  Serial.begin(9600);
  Serial.println("Inisialisasi Sistem Parkir...");
  
  // Inisialisasi Pin 
  pinMode(TRIG_PIN_MASUK, OUTPUT); pinMode(ECHO_PIN_MASUK, INPUT);
  pinMode(TRIG_PIN_KELUAR, OUTPUT); pinMode(ECHO_PIN_KELUAR, INPUT);
  pinMode(LED_HIJAU_MASUK, OUTPUT); pinMode(LED_MERAH_MASUK, OUTPUT);
  pinMode(LED_HIJAU_KELUAR, OUTPUT); pinMode(LED_MERAH_KELUAR, OUTPUT);
  
  servoMasuk.attach(SERVO_PIN_MASUK);
  servoKeluar.attach(SERVO_PIN_KELUAR);
  setPalangMasuk(false); 
  setPalangKeluar(false); 
  
  // --- FOKUS PERBAIKAN PADA OLED ---
  // 1. Pastikan I2C dimulai
  Wire.begin(); 
  
  // 2. Coba alamat 0x3C
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    // 3. Jika gagal, coba alamat 0x3D
    if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
        Serial.println(F(">>> ERROR: Gagal menemukan SSD1306. Cek koneksi A4/A5 atau alamat lain."));
        digitalWrite(LED_MERAH_MASUK, HIGH);
        digitalWrite(LED_MERAH_KELUAR, HIGH);
        // Tambahkan pengosongan buffer dan tampilan awal meskipun gagal inisialisasi
        display.clearDisplay();
        display.display(); 
        for(;;); 
    } else {
        Serial.println("OLED ditemukan di alamat 0x3D.");
    }
  } else {
    Serial.println("OLED ditemukan di alamat 0x3C.");
  }
  
  // 4. Perintah wajib setelah inisialisasi:
  display.display(); // Tampilkan buffer (harus hitam)
  delay(100);
  display.clearDisplay(); // Hapus buffer
  display.display(); // Tampilkan buffer kosong
  delay(500);
  
  // Tampilkan pesan konfirmasi di OLED
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("Sistem Parkir OK");
  display.display();
  delay(1000);
  
  updateOLED();
}

// ===================================================
// LOOP UTAMA
// ===================================================
void loop() {
  // ... (Loop tetap sama)
  long jarakMasuk = measureDistance(TRIG_PIN_MASUK, ECHO_PIN_MASUK);
  long jarakKeluar = measureDistance(TRIG_PIN_KELUAR, ECHO_PIN_KELUAR);
  
  Serial.print("Jarak Masuk: "); Serial.print(jarakMasuk); 
  Serial.print(" cm | Jarak Keluar: "); Serial.print(jarakKeluar); Serial.println(" cm");

  // Logika Palang Masuk
  if (jarakMasuk <= JARAK_DETEKSI_CM && !isPalangMasukOpen) { setPalangMasuk(true); }
  if (isPalangMasukOpen && (millis() - lastOpenTimeMasuk >= DELAY_LEWAT_MS)) {
    if (jarakMasuk > JARAK_DETEKSI_CM) { jumlahKendaraan++; setPalangMasuk(false); }
  }

  // Logika Palang Keluar
  if (jarakKeluar <= JARAK_DETEKSI_CM && !isPalangKeluarOpen && jumlahKendaraan > 0) { setPalangKeluar(true); }
  if (isPalangKeluarOpen && (millis() - lastOpenTimeKeluar >= DELAY_LEWAT_MS)) {
    if (jarakKeluar > JARAK_DETEKSI_CM) { jumlahKendaraan--; if (jumlahKendaraan < 0) jumlahKendaraan = 0; setPalangKeluar(false); }
  }
  
  delay(50);
}