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

LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo entryServo;
Servo exitServo;
int entryServoPin    = 9;
int exitServoPin     = 8;
int entryGateOpen    = 170;
int exitGateOpen     = 140;
int entryGateClosed  = 75;
int exitGateClosed   = 40;

int entryTrig1 = 2;
int entryEcho1 = 3;
int entryTrig2 = 4;
int entryEcho2 = A0;

int exitTrig1 = 7;
int exitEcho1 = A1;
int exitTrig2 = 13;
int exitEcho2 = A2;

int detectDist = 10;

int entryLedR = 10;
int entryLedG = 11;

int exitLedR = 5;
int exitLedG = 6;

int totalSlots     = 4;
int slotsAvailable = 4;

int entryIdle        = 0;
int entryWaitingS2   = 1;
int entryWaitingPass = 2;
int entryState       = 0;

int exitIdle        = 0;
int exitWaitingS4   = 1;
int exitWaitingPass = 2;
int exitState       = 0;

int entryCount = 0;
int exitCount  = 0;

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

void setEntryLED(int r, int g) {
  analogWrite(entryLedR, 255 - r);
  analogWrite(entryLedG, 255 - g);
}

void setExitLED(int r, int g) {
  analogWrite(exitLedR, 255 - r);
  analogWrite(exitLedG, 255 - g);
}

void updateLCD() {
  lcd.setCursor(0, 0);
  lcd.print("    WELCOME!    ");
  lcd.setCursor(0, 1);
  lcd.print("Slots Left: ");
  lcd.print(slotsAvailable);
  lcd.print("    ");
}

void showFull() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("    SORRY :(    ");
  lcd.setCursor(0, 1);
  lcd.print("  Parking Full  ");
  delay(3000);
  lcd.clear();
  updateLCD();
}

void setup() {
  pinMode(entryTrig1, OUTPUT);
  pinMode(entryEcho1, INPUT);
  pinMode(entryTrig2, OUTPUT);
  pinMode(entryEcho2, INPUT);
  pinMode(exitTrig1, OUTPUT);
  pinMode(exitEcho1, INPUT);
  pinMode(exitTrig2, OUTPUT);
  pinMode(exitEcho2, INPUT);

  pinMode(entryLedR, OUTPUT);
  pinMode(entryLedG, OUTPUT);
  pinMode(exitLedR, OUTPUT);
  pinMode(exitLedG, OUTPUT);

  setEntryLED(255, 0);
  setExitLED(255, 0);

  entryServo.attach(entryServoPin);
  exitServo.attach(exitServoPin);
  entryServo.write(entryGateClosed);
  exitServo.write(exitGateClosed);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("  ARDUINO  ");
  lcd.setCursor(0, 1);
  lcd.print("PARKING SYSTEM");
  delay(2000);
  lcd.clear();
  updateLCD();
}

void loop() {
  if (entryState == entryIdle) {
    if (getDistance(entryTrig1, entryEcho1) < detectDist) {
      entryCount++;
    } 
    else {
      entryCount = 0;
    }
    if (entryCount >= 3) {
      entryCount = 0;

      if (slotsAvailable <= 0) {
        showFull();
      } 
      else {
        entryServo.write(entryGateOpen);
        setEntryLED(0, 255);
        entryState = entryWaitingS2;
      }
    }
  }

  else if (entryState == entryWaitingS2) {
    if (getDistance(entryTrig2, entryEcho2) < detectDist) {
      entryState = entryWaitingPass;
    }
  }

  else if (entryState == entryWaitingPass) {
    if (getDistance(entryTrig2, entryEcho2) >= detectDist) {
      delay(100);
      entryServo.write(entryGateClosed);
      setEntryLED(255, 0);
      entryState = entryIdle;

      slotsAvailable--;
      if (slotsAvailable < 0) slotsAvailable = 0;
      updateLCD();
      Serial.println(slotsAvailable);
    }
  }

  if (exitState == exitIdle) {
    if (getDistance(exitTrig1, exitEcho1) < detectDist) {
      exitCount++;
    } else {
      exitCount = 0;
    }

    if (exitCount >= 3) {
      exitCount = 0;
      exitServo.write(exitGateOpen);
      setExitLED(0, 255);
      exitState = exitWaitingS4;
    }
  }

  else if (exitState == exitWaitingS4) {
    if (getDistance(exitTrig2, exitEcho2) < detectDist) {
      exitState = exitWaitingPass;
    }
  }

  else if (exitState == exitWaitingPass) {
    if (getDistance(exitTrig2, exitEcho2) >= detectDist) {
      delay(500);
      exitServo.write(exitGateClosed);
      setExitLED(255, 0);
      exitState = exitIdle;

      slotsAvailable++;
      if (slotsAvailable > totalSlots) slotsAvailable = totalSlots;
      updateLCD();
      Serial.println(slotsAvailable);
    }
  }
  delay(50);
}