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

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

// LED Pins
#define GREEN 2
#define YELLOW 3
#define RED 4

// Ultrasonic Pins
#define TRIG 6
#define ECHO 7

// Buzzer
#define BUZZER 8

long duration;
float distance;

void setup() {
  pinMode(GREEN, OUTPUT);
  pinMode(YELLOW, OUTPUT);
  pinMode(RED, OUTPUT);

  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  pinMode(BUZZER, OUTPUT);

  Serial.begin(9600);

  // OLED Initialization
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED failed");
    while (true);
  }

  display.clearDisplay();
  display.display();
}

float getDistance() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);

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

  duration = pulseIn(ECHO, HIGH);
  distance = duration * 0.034 / 2;

  return distance;
}

void loop() {

  // 🟢 GREEN
  digitalWrite(GREEN, HIGH);
  digitalWrite(YELLOW, LOW);
  digitalWrite(RED, LOW);
  delay(10000);

  // 🟡 YELLOW
  digitalWrite(GREEN, LOW);
  digitalWrite(YELLOW, HIGH);
  digitalWrite(RED, LOW);
  delay(10000);

  // 🔴 RED
  digitalWrite(GREEN, LOW);
  digitalWrite(YELLOW, LOW);
  digitalWrite(RED, HIGH);

  unsigned long startTime = millis();

  while (millis() - startTime < 10000) {

    distance = getDistance();
    Serial.println(distance);

    if (distance > 0 && distance < 10) {
      digitalWrite(BUZZER, HIGH);

      display.clearDisplay();
      display.setTextSize(2);
      display.setTextColor(WHITE);
      display.setCursor(5, 25);
      display.println("Red Light");
      display.setCursor(5, 45);
      display.println("Crossed");
      display.display();
    }
    else {
      digitalWrite(BUZZER, LOW);
      display.clearDisplay();
      display.display();
    }
  }

  digitalWrite(BUZZER, LOW);
}