#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo entryServo;
Servo exitServo;

// ---------------------------
// Parking slot sensors
// ---------------------------
#define SLOT1_TRIG 13
#define SLOT1_ECHO 12

#define SLOT2_TRIG 14
#define SLOT2_ECHO 27

#define SLOT3_TRIG 26
#define SLOT3_ECHO 25

#define SLOT4_TRIG 33
#define SLOT4_ECHO 32

// ---------------------------
// Entry and Exit sensors
// ---------------------------
#define ENTRY_TRIG 5
#define ENTRY_ECHO 18

#define EXIT_TRIG 19
#define EXIT_ECHO 23

// ---------------------------
// Servo pins
// ---------------------------
#define ENTRY_SERVO 4
#define EXIT_SERVO 16

// Parking distance threshold
#define PARKING_DISTANCE 10

// ---------------------------
// Read distance
// ---------------------------
long getDistance(int trigPin, int echoPin)
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000);

  if (duration == 0)
    return 999;

  long distance = duration * 0.034 / 2;

  return distance;
}

// ---------------------------
// Setup
// ---------------------------
void setup()
{
  Serial.begin(115200);

  // Slot sensors
  pinMode(SLOT1_TRIG, OUTPUT);
  pinMode(SLOT1_ECHO, INPUT);

  pinMode(SLOT2_TRIG, OUTPUT);
  pinMode(SLOT2_ECHO, INPUT);

  pinMode(SLOT3_TRIG, OUTPUT);
  pinMode(SLOT3_ECHO, INPUT);

  pinMode(SLOT4_TRIG, OUTPUT);
  pinMode(SLOT4_ECHO, INPUT);

  // Entry sensor
  pinMode(ENTRY_TRIG, OUTPUT);
  pinMode(ENTRY_ECHO, INPUT);

  // Exit sensor
  pinMode(EXIT_TRIG, OUTPUT);
  pinMode(EXIT_ECHO, INPUT);

  // Servos
  entryServo.attach(ENTRY_SERVO);
  exitServo.attach(EXIT_SERVO);

  entryServo.write(0);
  exitServo.write(0);

  // LCD
  lcd.init();
  lcd.backlight();

  lcd.setCursor(0, 0);
  lcd.print("SMART PARKING");

  lcd.setCursor(0, 1);
  lcd.print("SYSTEM READY");

  delay(2000);
  lcd.clear();
}

// ---------------------------
// Main loop
// ---------------------------
void loop()
{
  long slot1 = getDistance(SLOT1_TRIG, SLOT1_ECHO);
  delay(50);

  long slot2 = getDistance(SLOT2_TRIG, SLOT2_ECHO);
  delay(50);

  long slot3 = getDistance(SLOT3_TRIG, SLOT3_ECHO);
  delay(50);

  long slot4 = getDistance(SLOT4_TRIG, SLOT4_ECHO);

  // Check occupied slots
  bool occupied1 = slot1 < PARKING_DISTANCE;
  bool occupied2 = slot2 < PARKING_DISTANCE;
  bool occupied3 = slot3 < PARKING_DISTANCE;
  bool occupied4 = slot4 < PARKING_DISTANCE;

  int occupied = 0;

  if (occupied1) occupied++;
  if (occupied2) occupied++;
  if (occupied3) occupied++;
  if (occupied4) occupied++;

  int available = 4 - occupied;

  // ---------------------------
  // LCD display
  // ---------------------------

  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("FREE:");
  lcd.print(available);

  lcd.setCursor(9, 0);
  lcd.print("USED:");
  lcd.print(occupied);

  lcd.setCursor(0, 1);

  lcd.print("S1");
  lcd.print(occupied1 ? "X " : "O ");

  lcd.print("S2");
  lcd.print(occupied2 ? "X " : "O ");

  lcd.print("S3");
  lcd.print(occupied3 ? "X " : "O ");

  lcd.print("S4");
  lcd.print(occupied4 ? "X" : "O");

  // ---------------------------
  // Serial monitor
  // ---------------------------

  Serial.println("--------------------");

  Serial.print("Slot 1: ");
  Serial.print(slot1);
  Serial.println(" cm");

  Serial.print("Slot 2: ");
  Serial.print(slot2);
  Serial.println(" cm");

  Serial.print("Slot 3: ");
  Serial.print(slot3);
  Serial.println(" cm");

  Serial.print("Slot 4: ");
  Serial.print(slot4);
  Serial.println(" cm");

  Serial.print("Available Slots: ");
  Serial.println(available);

  // ---------------------------
  // Entry gate
  // ---------------------------

  long entryDistance =
      getDistance(ENTRY_TRIG, ENTRY_ECHO);

  if (entryDistance < PARKING_DISTANCE)
  {
    if (available > 0)
    {
      Serial.println("Vehicle detected at ENTRY");
      Serial.println("Gate OPEN");

      entryServo.write(90);

      delay(3000);

      entryServo.write(0);

      Serial.println("Gate CLOSED");

      delay(1000);
    }
    else
    {
      Serial.println("PARKING FULL");

      lcd.clear();

      lcd.setCursor(0, 0);
      lcd.print("PARKING FULL");

      lcd.setCursor(0, 1);
      lcd.print("ENTRY DENIED");

      delay(2000);
    }
  }

  // ---------------------------
  // Exit gate
  // ---------------------------

  long exitDistance =
      getDistance(EXIT_TRIG, EXIT_ECHO);

  if (exitDistance < PARKING_DISTANCE)
  {
    Serial.println("Vehicle detected at EXIT");

    exitServo.write(90);

    delay(3000);

    exitServo.write(0);

    Serial.println("EXIT GATE CLOSED");

    delay(1000);
  }

  delay(500);
}