#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>

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

// Pin definitions untuk Arduino Nano
const int irEntry = 2;     // Sensor IR masuk
const int irExit = 3;      // Sensor IR keluar
const int servoPin = 5;    // Servo motor
const int buzzer = 6;      // Buzzer
const int ledRed = 7;      // LED Merah
const int ledGreen = 8;    // LED Hijau
const int buttonSet = 9;   // Tombol set kapasitas

// EEPROM Addresses
const int addrMaxCapacity = 0;
const int addrCarCount = 1;

// Variables
int currentCount = 0;
int maxCapacity = 100;
bool gateOpen = false;
bool isFull = false;

// Variables untuk logika urutan sensor
bool waitingForExit = false;
bool waitingForEntry = false;
unsigned long gateOpenTime = 0;
const unsigned long sequenceTimeout = 15000;

// Timing variables
unsigned long lastDebounceTime = 0;
unsigned long fullAlarmTime = 0;
const unsigned long debounceDelay = 100;
const unsigned long alarmInterval = 1000;

Servo gateServo;

void setup() {
  // Initialize pins
  pinMode(irEntry, INPUT_PULLUP);
  pinMode(irExit, INPUT_PULLUP);
  pinMode(buzzer, OUTPUT);
  pinMode(ledRed, OUTPUT);
  pinMode(ledGreen, OUTPUT);
  pinMode(buttonSet, INPUT_PULLUP);
  
  // Initialize Servo
  gateServo.attach(servoPin);
  gateServo.write(0);
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();
  
  // Load data from EEPROM
  loadFromEEPROM();
  
  // Startup display
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("PARKING SYSTEM");
  lcd.setCursor(0, 1);
  lcd.print("Quick Close Mode");
  delay(2000);
  
  updateLCD();
  
  Serial.begin(9600);
  Serial.println("=== QUICK CLOSE PARKING SYSTEM ===");
  printDebugInfo();
}

void loop() {
  // Check set capacity button (hold 3 seconds)
  if (digitalRead(buttonSet) == LOW) {
    delay(3000);
    if (digitalRead(buttonSet) == LOW) {
      setCapacityMode();
    }
  }

  // Baca sensor
  bool entrySensor = (digitalRead(irEntry) == LOW);
  bool exitSensor = (digitalRead(irExit) == LOW);

  // **LOGIKA MOBIL MASUK: Entry → Buka Gerbang → Exit → Tutup Gerbang**
  if (entrySensor && !gateOpen && !waitingForExit && !isFull) {
    if (millis() - lastDebounceTime > debounceDelay) {
      Serial.println("ENTRY sensor - Opening gate for ENTRY");
      openGate();
      waitingForExit = true;
      lastDebounceTime = millis();
    }
  }

  // **LOGIKA MOBIL KELUAR: Exit → Buka Gerbang → Entry → Tutup Gerbang**
  if (exitSensor && !gateOpen && !waitingForEntry) {
    if (millis() - lastDebounceTime > debounceDelay) {
      Serial.println("EXIT sensor - Opening gate for EXIT");
      openGate();
      waitingForEntry = true;
      lastDebounceTime = millis();
    }
  }

  // **Setelah gate terbuka untuk MASUK, tunggu sensor EXIT lalu TUTUP**
  if (waitingForExit && gateOpen && exitSensor) {
    if (millis() - lastDebounceTime > debounceDelay) {
      // MOBIL MASUK BERHASIL
      currentCount++;
      Serial.print("CAR ENTERED - Count: ");
      Serial.println(currentCount);
      
      if (currentCount >= maxCapacity) {
        isFull = true;
        fullAlarmTime = millis();
        Serial.println("PARKING FULL!");
      }
      
      saveToEEPROM();
      updateLCD();
      waitingForExit = false;
      
      // **TUTUP GERBANG SETELAH MOTOR LEWAT SENSOR EXIT**
      delay(500); // Beri jeda sebentar sebelum tutup
      closeGate();
      lastDebounceTime = millis();
    }
  }

  // **Setelah gate terbuka untuk KELUAR, tunggu sensor ENTRY lalu TUTUP**
  if (waitingForEntry && gateOpen && entrySensor) {
    if (millis() - lastDebounceTime > debounceDelay) {
      // MOBIL KELUAR BERHASIL
      if (currentCount > 0) {
        currentCount--;
      }
      
      if (isFull && currentCount < maxCapacity) {
        isFull = false;
        noTone(buzzer);
        Serial.println("Parking available again");
      }
      
      saveToEEPROM();
      updateLCD();
      Serial.print("CAR EXITED - Count: ");
      Serial.println(currentCount);
      waitingForEntry = false;
      
      // **TUTUP GERBANG SETELAH MOTOR LEWAT SENSOR ENTRY**
      delay(500); // Beri jeda sebentar sebelum tutup
      closeGate();
      lastDebounceTime = millis();
    }
  }

  // **Timeout: Jika dalam 3 detik tidak ada sensor kedua terdeteksi, TUTUP GERBANG**
  if ((waitingForExit || waitingForEntry) && gateOpen) {
    if (millis() - gateOpenTime > sequenceTimeout) {
      Serial.println("Sequence timeout - Closing gate");
      if (waitingForExit) {
        Serial.println("Entry sequence cancelled");
        waitingForExit = false;
      }
      if (waitingForEntry) {
        Serial.println("Exit sequence cancelled");
        waitingForEntry = false;
      }
      closeGate();
    }
  }

  // **Full parking alarm**
  if (isFull) {
    if (millis() - fullAlarmTime >= alarmInterval) {
      tone(buzzer, 1500, 500);
      fullAlarmTime = millis();
      digitalWrite(ledRed, !digitalRead(ledRed));
    }
  } else {
    digitalWrite(ledRed, LOW);
  }

  // Debug output
  static unsigned long lastDebug = 0;
  if (millis() - lastDebug > 3000) {
    printSensorDebug();
    lastDebug = millis();
  }
  
  delay(50);
}

void openGate() {
  if (!gateOpen) {
    Serial.println("Opening GATE");
    gateOpen = true;
    gateOpenTime = millis();
    digitalWrite(ledRed, LOW);
    digitalWrite(ledGreen, HIGH);
    tone(buzzer, 1000, 200);
    
    // Smooth gate opening
    for (int pos = 0; pos <= 90; pos += 3) {
      gateServo.write(pos);
      delay(25);
    }
    Serial.println("Gate OPENED");
  }
}

void closeGate() {
  if (gateOpen) {
    Serial.println("Closing GATE");
    gateOpen = false;
    digitalWrite(ledGreen, LOW);
    if (!isFull) {
      digitalWrite(ledRed, HIGH);
    }
    tone(buzzer, 500, 200);
    
    // Smooth gate closing
    for (int pos = 90; pos >= 0; pos -= 3) {
      gateServo.write(pos);
      delay(25);
    }
    Serial.println("Gate CLOSED");
  }
}

void updateLCD() {
  lcd.clear();
  
  // Line 1: Capacity and Counter
  lcd.setCursor(0, 0);
  lcd.print("Cap:");
  lcd.print(maxCapacity);
  lcd.setCursor(7, 0);
  lcd.print("Now:");
  lcd.print(currentCount);
  
  // Line 2: Status
  lcd.setCursor(0, 1);
  if (isFull) {
    lcd.print(">>> FULL <<<");
  } else if (gateOpen) {
    if (waitingForExit) {
      lcd.print("WAIT EXIT SENSOR");
    } else if (waitingForEntry) {
      lcd.print("WAIT ENTRY SENSOR");
    } else {
      lcd.print(">>> OPEN  <<<");
    }
  } else {
    lcd.print(">>> CLOSED <<<");
  }
  
  // Available spaces
  lcd.setCursor(13, 0);
  int spaces = maxCapacity - currentCount;
  if (spaces > 0) {
    lcd.print("P:");
    lcd.print(spaces);
  } else {
    lcd.print("FUL");
  }
}

void setCapacityMode() {
  // ... (sama seperti sebelumnya)
}

void updateCapacityDisplay(int capacity) {
  // ... (sama seperti sebelumnya)
}

void loadFromEEPROM() {
  // ... (sama seperti sebelumnya)
}

void saveToEEPROM() {
  // ... (sama seperti sebelumnya)
}

void printDebugInfo() {
  Serial.print("Max Capacity: ");
  Serial.println(maxCapacity);
  Serial.print("Current Count: ");
  Serial.println(currentCount);
  Serial.print("isFull: ");
  Serial.println(isFull);
  Serial.println("Quick Close Mode: Gate closes after second sensor");
  Serial.println("Ready for operation...");
}

void printSensorDebug() {
  Serial.print("Sensors - Entry: ");
  Serial.print(digitalRead(irEntry));
  Serial.print(" | Exit: ");
  Serial.print(digitalRead(irExit));
  Serial.print(" | Gate: ");
  Serial.print(gateOpen ? "OPEN" : "CLOSED");
  Serial.print(" | WaitExit: ");
  Serial.print(waitingForExit);
  Serial.print(" | WaitEntry: ");
  Serial.print(waitingForEntry);
  Serial.print(" | Count: ");
  Serial.println(currentCount);
}