#include <Servo.h> // Include the Servo library to control servo motors

// Pin configuration
const int YELLOW_PIN = 12; // Set the yellow LED to pin 12
const int PUSH_BUTTON = 4; // Set the push button pin to pin 4
const int TRIG = 6;        // Set the TRIG pin of the ultrasonic sensor to pin 6
const int ECHO = 5;        // Set the ECHO pin of the ultrasonic sensor to pin 5

// Create servo objects
Servo myServo1; // Create a global variable for the first servo
Servo myServo2; // Create a global variable for the second servo

long previousMillis = 0; // Initialize previousMillis to 0 for timing operations
long interval = 500;     // Set interval for buzzer duration (currently unused in this code)

void setup() {
  // Configure pins and initialize system
  pinMode(YELLOW_PIN, OUTPUT); // Set the yellow LED pin as output
  pinMode(TRIG, OUTPUT);       // Set the TRIG pin of the ultrasonic sensor as output
  pinMode(ECHO, INPUT);        // Set the ECHO pin of the ultrasonic sensor as input
  Serial.begin(9600);          // Start serial communication
  Serial.println("Train Crossing Active!"); // Print a startup message to the serial monitor
}

// This function calculates the distance based on ultrasonic sensor readings
long getDistance() {
  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) { //if the echo pin is activated start reading distances to teh object
    endTime = micros();
    if (millis() - timeout > 50) return -1; // Exit if timeout
  }

  // Calculate the duration in microseconds based off from the endtime - starttime
  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; //returns the distance to the object
}

// This function turns on the yellow LED when it is calld
void yellow_light() {
  digitalWrite(YELLOW_PIN, HIGH); // Activate the yellow LED
}

// This function turns off the yellow LED and ensures no other LEDs or components are activated
void no_light() {
  digitalWrite(YELLOW_PIN, LOW); // Deactivate the yellow LED
}

bool buttonPressed = false; // Variable to track if the push button has been pressed

// The loop function runs repeatedly and contains the main program logic
void loop() {
  // Check if the push button is pressed and the system is not already activated
  if (digitalRead(PUSH_BUTTON) == HIGH && !buttonPressed) { 
    buttonPressed = true; // Mark the system as activated
  }

  // If the system is activated, perform the following logic
  if (buttonPressed) {
    // Get the distance of the object detected by the ultrasonic sensor
    long distance = getDistance();

    // Perform actions based on the detected distance
    if (distance >= 90) {
      // If the object is far away (90 cm or more), turn off the yellow LED
      no_light();
    } else if (distance > 40 && distance < 90) {
      // If the object is at a medium distance (between 40 cm and 90 cm), do nothing
      no_light();
    } else if (distance > 16 && distance <= 40) {
      // If the object is closer (between 16 cm and 40 cm), turn on the yellow LED
      yellow_light();
    } else if (distance <= 15) {
      // If the object is very close (15 cm or less), turn off the yellow LED
      no_light();
    }
    // Add additional logic here if needed, such as triggering a buzzer or moving servos
  }
}