#include <Servo.h>

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;



const int servo1Pin = 9;
const int servo2Pin = 10; // the number of the 2nd Crossong Guard Servo pin



// create servo objects
Servo myServo1;
Servo myServo2;

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

void setup() {



  // set inital crossing guard positions
  CrossingGuardsUp();

  myServo1.attach(servo1Pin);
  myServo2.attach(servo2Pin);
 
  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 CrossingGuardsUp(){
  // Bring the Crossing Guard arms to the up state
  myServo1.write(90);
  myServo2.write(80);
}


//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;
}



bool buttonPressed = false;
// delay(flashDelay)
void loop() 
{
  if (digitalRead(PUSH_BUTTON) == HIGH && !buttonPressed) {
    buttonPressed = true; // Track that the button was pressed
  }
  if(buttonPressed)
  {
    
  // put your main code here, to run repeatedly:
    long distance = getDistance();
    // start the crossing guard actions
    if(distance >=90)
    {
      //do nothing do nto put the crossing guards down here
    }
    else if(distance > 40 && distance < 90)
    {
     
      CrossingGuardsUp();

    }
    else if(distance > 16 && distance < 40)
    {
      // flash the LEDs x number of times for YY milliseconds
     //do nothing do not piut the crossing guards up/do
    }
    else if(distance <=15) //place the crossing guards down to indicate that there is a train moving

    {
      CrossingGuardsDown();
      // flash the LEDs x number of times for YY milliseconds
      if(millis() - previousMillis > interval)
      {
        previousMillis = millis(); // Reset the timer
      }
    }
    // Check if the buzzer should be turned off
  }

}