#include <Servo.h>

// Constants for component pins
const int buttonPin = 2;       // Pushbutton pin
const int ledPin1L = 13;       // 1st Indicator LED Left pin
const int ledPin1R = 12;       // 1st Indicator LED Right pin
const int ledPin2L = 11;       // 2nd Indicator LED Left pin
const int ledPin2R = 10;       // 2nd Indicator LED Right pin
const int servo1Pin = 7;       // 1st Crossing Guard Servo pin
const int servo2Pin = 6;       // 2nd Crossing Guard Servo pin
const int buzzerPin = 5;       // Buzzer pin (optional)

const int RED_PIN = 16;
const int YELLOW_PIN = 17;
const int GREEN_PIN = 18;

const int TRIG = 14;           // Ultrasonic sensor TRIG pin
const int ECHO = 15;           // Ultrasonic sensor ECHO pin

// Variables for debounce
bool buttonPressed = false;
bool crossingActive = false;

// Servo objects
Servo myServo1;
Servo myServo2;


void setup() {
  pinMode(buttonPin, INPUT);
  pinMode(ledPin1L, OUTPUT);
  pinMode(ledPin1R, OUTPUT);
  pinMode(ledPin2L, OUTPUT);
  pinMode(ledPin2R, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(YELLOW_PIN, OUTPUT);
  pinMode(RED_PIN, OUTPUT);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  // Attach servos and set initial position
  myServo1.attach(servo1Pin);
  myServo2.attach(servo2Pin);
  CrossingGuardsUp();

  Serial.begin(9600);
  Serial.println("Train Crossing Active!");
}

// Function to move crossing guards down
void CrossingGuardsDown() {
  myServo1.write(180);
  myServo2.write(0);
}

// Function to move crossing guards up
void CrossingGuardsUp() {
  myServo1.write(90);
  myServo2.write(80);
}

// Function to flash LEDs
void FlashLEDs(int flashTimes, int flashDelay, bool beep) {
  for (int i = 0; i < flashTimes; i++) {
    digitalWrite(ledPin1L, HIGH);
    digitalWrite(ledPin2R, HIGH);
    digitalWrite(ledPin2L, LOW);
    digitalWrite(ledPin1R, LOW);
    if (beep) digitalWrite(buzzerPin, HIGH);
    delay(flashDelay);

    digitalWrite(buzzerPin, LOW);
    digitalWrite(ledPin1L, LOW);
    digitalWrite(ledPin2R, LOW);
    digitalWrite(ledPin2L, HIGH);
    digitalWrite(ledPin1R, HIGH);
    delay(flashDelay);
  }
  digitalWrite(ledPin1L, LOW);
  digitalWrite(ledPin2R, LOW);
  digitalWrite(ledPin2L, LOW);
  digitalWrite(ledPin1R, LOW);
}

// Function to measure distance
long getDistance() {
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  unsigned long startTime = 0;
  unsigned long endTime = 0;
  unsigned long timeout = millis();

  while (digitalRead(ECHO) == LOW) {
    startTime = micros();
    if (millis() - timeout > 50) return -1;
  }
  while (digitalRead(ECHO) == HIGH) {
    endTime = micros();
    if (millis() - timeout > 50) return -1;
  }

  long duration = endTime - startTime;
  return duration / 58;
}

// Light control functions
void greenLight() {
  digitalWrite(GREEN_PIN, HIGH);
  digitalWrite(YELLOW_PIN, LOW);
  digitalWrite(RED_PIN, LOW);
}

void red_light() {
  digitalWrite(GREEN_PIN, LOW);
  digitalWrite(YELLOW_PIN, LOW);
  digitalWrite(RED_PIN, HIGH);
}

void yellow_light() {
  digitalWrite(GREEN_PIN, LOW);
  digitalWrite(YELLOW_PIN, HIGH);
  digitalWrite(RED_PIN, LOW);
}

void no_light() {
  digitalWrite(GREEN_PIN, LOW);
  digitalWrite(YELLOW_PIN, LOW);
  digitalWrite(RED_PIN, LOW);
}

void loop() {
  int buttonState = digitalRead(buttonPin);
  long distance = getDistance();

  if (buttonState == HIGH && !crossingActive) {
    crossingActive = true;
    FlashLEDs(7, 200, true);
  } else if (buttonState == LOW && crossingActive) {
    crossingActive = false;
  }

  if (crossingActive) {
    if (distance >= 90) {
      no_light();
      CrossingGuardsUp();
    } else if (distance > 23 && distance < 90) {
      greenLight();
      CrossingGuardsUp();
      FlashLEDs(5, 200, false);
    } else if (distance > 16 && distance <= 22) {
      yellow_light();
      CrossingGuardsDown();
      FlashLEDs(20, 200, true);
    } else if (distance <= 15) {
      red_light();
      CrossingGuardsDown();
      tone(buzzerPin, 500);
    
    }


  }
}