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

// Ultrasonic sensor pins
const int trigPinIn = 2;
const int echoPinIn = 3;
const int trigPinOut = 4;
const int echoPinOut = 5;

// Servo motor pin
const int servoPin = 6;

// Parking space variables
int currentCount = 0;
const int maxSpaces = 4; // Tunable max parking spaces
const int carDetectDistance = 5; // cm

Servo gateServo;
LiquidCrystal_I2C lcd(0x27, 16, 2); // Common I2C address: 0x27 or 0x3F

// Function to measure distance from ultrasonic sensor
long getDistance(int trigPin, int echoPin) {
  /* 
    IO: trigPin (output), echoPin (input)
    Working: Sends ultrasonic pulse and measures echo time to calculate distance
    Returns: Distance in cm (long), -1 if timeout/error
  */
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000); // Timeout 30ms
  if (duration == 0) {
    return -1; // Error / no echo
  }
  long distance = duration * 0.034 / 2;
  return distance;
}

// Function to open gate
void openGate() {
  gateServo.write(90); // Adjust angle for your setup
}

// Function to close gate
void closeGate() {
  gateServo.write(0); // Adjust angle for your setup
}

// Function to update LCD
void updateLcd(const char* line1, const char* line2) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(line1);
  lcd.setCursor(0, 1);
  lcd.print(line2);
}

void setup() {
  Serial.begin(9600);

  pinMode(trigPinIn, OUTPUT);
  pinMode(echoPinIn, INPUT);
  pinMode(trigPinOut, OUTPUT);
  pinMode(echoPinOut, INPUT);

  gateServo.attach(servoPin);
  closeGate();

  lcd.init();
  lcd.backlight();
  updateLcd("Smart Parking", "System Ready");
  delay(2000);
  updateLcd("Car Count:", "0");
}

void loop() {
  long distanceIn = getDistance(trigPinIn, echoPinIn);
  long distanceOut = getDistance(trigPinOut, echoPinOut);

  // Car entering
  if (distanceIn != -1 && distanceIn <= carDetectDistance) {
    if (currentCount < maxSpaces) {
      openGate();
      updateLcd("Welcome!", "Gate Opening...");
      delay(3000); // Allow car to pass (adjust as needed)
      closeGate();
      currentCount++;
      char buffer[16];
      sprintf(buffer, "Car Count: %d", currentCount);
      updateLcd("Entry Success", buffer);
      Serial.print("Car Entered. Current Count: ");
      Serial.println(currentCount);
    } else {
      updateLcd("Parking Full!", "Access Denied");
      Serial.println("Parking Full! Gate not opened.");
    }
  }

  // Car exiting
  if (distanceOut != -1 && distanceOut <= carDetectDistance) {
    openGate();
    updateLcd("Goodbye!", "Drive Safely");
    delay(10000); // Gate stays open for 10 sec
    closeGate();
    if (currentCount > 0) {
      currentCount--;
    }
    char buffer[16];
    sprintf(buffer, "Car Count: %d", currentCount);
    updateLcd("Exit Success", buffer);
    Serial.print("Car Exited. Current Count: ");
    Serial.println(currentCount);
  }

  delay(200); // Small delay to avoid false triggers
}