#include <Servo.h>
const int buttonPin = 2; // the number of the pushbutton pin
const int ledPin1L = 13; // the number of the 1st Indicator LED Left pin
const int ledPin1R = 12; // the number of the 1st Indicator LED Right pin
const int ledPin2L = 11; // the number of the 2nd Indicator LED Left pin
const int ledPin2R = 10; // the number of the 2nd Indicator LED Right pin
const int servo1Pin = 7; // the number of the 1st Crossing Guard Servo pin
const int servo2Pin = 6; // the number of the 2nd Crossong Guard Servo pin

const int GREEN_PIN = 11;
const int YELLOW_PIN = 12;
const int RED_PIN = 13;

int buttonState = LOW; // variable for reading the pushbutton status
const int BUZZER_BUTTON = 10;

const int PUSH_BUTTON = 4;
const int TRIG = 6; //..We are setting the TRIG on the ultrasonic sensor to port 23
const int ECHO = 5;


long previousMillis = 0;
long interval = 500;        // First length of time I want the buzzer to stay on set tone

void setup() {
  // initialize the LED pins as an output:
  pinMode(ledPin1L, OUTPUT);
  pinMode(ledPin1R, OUTPUT);
  pinMode(ledPin2L, OUTPUT);
  pinMode(ledPin2R, OUTPUT);

  // initialize the servo pins
  myServo1.attach(servo1Pin);
  myServo2.attach(servo2Pin);

  // set inital crossing guard positions
  CrossingGuardsUp();

  // put your setup code here, to run once:
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(YELLOW_PIN, OUTPUT);
  pinMode(RED_PIN, OUTPUT);
  pinMode(BUZZER_BUTTON, OUTPUT);
  
  pinMode(TRIG, OUTPUT);    // TRIG pin as output
  pinMode(ECHO, INPUT);     // ECHO pin as input
  Serial.begin(9600);
  Serial.println("Train Crossing Active!");
}

void CrossingGuardsDown()
{
  // Bring the Crossing Guard arms to the down state
  myServo1.write(180);
  myServo2.write(0);
}




void FlashLEDs(int flashTimes, int flashDelay, bool beep) {
  // Flash the crossing guard warning indicators for xx period of time
  int ctr1 = 0;
  for (ctr1 = 0; ctr1 < flashTimes; ctr1 += 1) {
  // Outer lights on, inner lights off
  digitalWrite(ledPin1L, HIGH);
  digitalWrite(ledPin2R, HIGH);
  digitalWrite(ledPin2L, LOW);
  digitalWrite(ledPin1R, LOW);
 
  // if beep is true buzz the buzzer only on this cycle
  if (beep == true) {
    digitalWrite(buzzerPin, HIGH); 
  }
  delay(flashDelay);
  // if beep is true stop the buzzer
  if (beep == true) {
    digitalWrite(buzzerPin, LOW);
  }
  // Inner lights on, outer lights off
  digitalWrite(ledPin2L, HIGH);
  digitalWrite(ledPin1R, HIGH);
  digitalWrite(ledPin1L, LOW);
  digitalWrite(ledPin2R, LOW);
  delay(flashDelay);
  }
  // all lights off at end of sequence
  digitalWrite(ledPin1L, LOW);
  digitalWrite(ledPin2R, LOW);
  digitalWrite(ledPin2L, LOW);
  digitalWrite(ledPin1R, LOW);
}



//define a function that is called get distance gets distance of ultrasonic wave that is sent
long getDistance()// #define a function that is called get distanc
{
  digitalWrite(TRIG, HIGH);  // Start the ultrasonic pulse
  delayMicroseconds(10);     // Send a 10-microsecond pulse
  digitalWrite(TRIG, LOW);   // Stop the pulse

  unsigned long startTime = 0;
  unsigned long endTime = 0;

  // Wait for ECHO to go HIGH and record the start time
  unsigned long timeout = millis();
  while (digitalRead(ECHO) == LOW) {
    startTime = micros();
    if (millis() - timeout > 50) return -1; // Exit if timeout (no response)
  }

  // Wait for ECHO to go LOW and record the end time
  timeout = millis();
  while (digitalRead(ECHO) == HIGH) {
    endTime = micros();
    if (millis() - timeout > 50) return -1; // Exit if timeout
  }

  // Calculate the duration in microseconds
  long duration = endTime - startTime;

  // Convert duration to distance in centimeters
  long distance_cm = duration / 58; // 58 us per cm for sound in air
  
  return distance_cm;
}


//This is what the output should be when the function green light is to be run 
void greenLight()
{
    digitalWrite(GREEN_PIN,  HIGH); //turn will turn on green light
    digitalWrite(YELLOW_PIN,  LOW); //turn the yellow led off
    digitalWrite(RED_PIN, LOW); //turn the red led off
    
}

void yellow_light() 
{
        
    digitalWrite(GREEN_PIN, LOW);   // Turn off green light
    digitalWrite(YELLOW_PIN, HIGH);  // Turn on yellow light
    digitalWrite(RED_PIN, LOW);      // Turn off red light
    
    
}


void red_light() 
{
    digitalWrite(GREEN_PIN, LOW);     // Turn off green light
    digitalWrite(YELLOW_PIN, LOW);    // Turn off yellow light
    digitalWrite(RED_PIN, HIGH);       // Turn on red light
    
    
}

void no_light() 
{
        // Ensure all lights and buzzer are off
    digitalWrite(GREEN_PIN, LOW);   
    digitalWrite(YELLOW_PIN, LOW);  
    digitalWrite(RED_PIN, LOW);     
  
}


// delay(flashDelay)
void loop() 
{

  //
  static unsigned long lastDebounceTime = 0;
  const unsigned long debounceDelay = 50; // debounce delay
  int reading = digitalRead(buttonPin);
  
  if (reading != buttonState) {
    lastDebounceTime = millis();
  }
  
  if ((millis() - lastDebounceTime) > debounceDelay) {
    buttonState = reading;
  }
  
  if (buttonState == HIGH) {
    FlashLEDs(7, 200, true);  // Flash LEDs on button press
    
    long distance = getDistance();
    
    if (distance >= 90) {
      noLight();
      CrossingGuardsUp();  // Ensure guards are up if distance is safe
    } else if (distance > 23 && distance < 90) {
      greenLight();
      CrossingGuardsUp();
      FlashLEDs(5, 200, false); // Safe, flash LEDs without beep
    } else if (distance > 16 && distance <= 22) {
      yellowLight();
      CrossingGuardsDown();
      FlashLEDs(20, 200, true); // Warning state
    } else if (distance <= 15) {
      redLight();
      CrossingGuardsDown();
      
      // Use non-blocking timing for buzzer
      if (millis() - previousMillis > interval) {
        tone(buzzerPin, 500);
        previousMillis = millis();
      }
    }
  } else {
    noLight();
    CrossingGuardsUp();  // Guards up if button not pressed
  }

}