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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int TRIG_PIN  = 12;
const int ECHO_PIN  = 14;
const int SERVO_PIN = 13;

Servo barrierServo;

const int EMPTY_LANE_DISTANCE = 100;

void setup() {
  Serial.begin(115200);
  
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  barrierServo.attach(SERVO_PIN);
  barrierServo.write(0); 
  
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(String("OLED failed to initialize"));
    for(;;);
  }
  
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(15, 20);
  display.println("PARKING SIMULATOR");
  display.setCursor(30, 40);
  display.println("READY... SYSTEM LIVE");
  display.display();
  delay(2000);
}

void loop() {
  long duration;
  int distance;
  
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;

  if (distance > 0 && distance < (EMPTY_LANE_DISTANCE - 10)) {
    
    int vehicleHeight = EMPTY_LANE_DISTANCE - distance;
    
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(0, 0);
    
    if (vehicleHeight <= 30) {
      display.println("CAR");
      display.setTextSize(1);
      display.println("\n-> Go LEFT\n-> Route to Zone A");
    } 
    else if (vehicleHeight > 30 && vehicleHeight <= 60) {
      display.println("VAN");
      display.setTextSize(1);
      display.println("\n-> Go STRAIGHT\n-> Route to Zone B");
    } 
    else {
      display.println("HGV");
      display.setTextSize(1);
      display.println("\n-> Go RIGHT\n-> Route to Zone C");
    }
    
    display.display();
    delay(4000);

    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(10, 15);
    display.println("GATE OPEN");
    display.setTextSize(1);
    display.setCursor(15, 45);
    display.println("Drive safely...");
    display.display();
    
    barrierServo.write(90);
    delay(5000);
    
    barrierServo.write(0);
    
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(10, 25);
    display.println("WAITING FOR VEHICLE");
    display.display();
  }
  
  delay(200);
}