#include <ESP32Servo.h>

Servo leftGateServo;  // Servo for the left gate
Servo rightGateServo; // Servo for the right gate

// IR sensor pins
const int trainIR1 = 32;   // Train detection IR sensor 1
const int trainIR2 = 33;   // Train detection IR sensor 2
const int vehicleIR1 = 25; // Vehicle detection IR sensor 1
const int vehicleIR2 = 26; // Vehicle detection IR sensor 2

// LED and buzzer pins
const int redLED1 = 27;
const int redLED2 = 14;
const int greenLED1 = 12;
const int greenLED2 = 13;
const int buzzer = 4;

// Servo gate positions
const int gateClosedPosition = 0;  // Closed gate position
const int gateOpenPosition = 90;   // Open gate position

void setup() {
  pinMode(trainIR1, INPUT);
  pinMode(trainIR2, INPUT);
  pinMode(vehicleIR1, INPUT);
  pinMode(vehicleIR2, INPUT);
  
  pinMode(redLED1, OUTPUT);
  pinMode(redLED2, OUTPUT);
  pinMode(greenLED1, OUTPUT);
  pinMode(greenLED2, OUTPUT);
  pinMode(buzzer, OUTPUT);
  digitalWrite(buzzer, LOW);
  
  leftGateServo.attach(5);  // Assign pin for left gate servo
  rightGateServo.attach(18); // Assign pin for right gate servo
  
  // Open gates at the start
  // openGates();
}

void loop() {
  bool trainDetected = digitalRead(trainIR1) == LOW || digitalRead(trainIR2) == LOW;
  bool vehicleDetected = digitalRead(vehicleIR1) == LOW || digitalRead(vehicleIR2) == LOW;
  trainDetected = 1; //test
  vehicleDetected = 0; //test

  if (trainDetected) {
    // Train approaching
    closeGates(vehicleDetected); // Check for vehicle before closing
    delay(3000); //test
    openGates(); //test
    delay(3000); //test
  } else {
    // No train, keep gates open
    openGates();
  }
}

void closeGates(bool vehicleDetected) {
  if (vehicleDetected) {
    // If vehicle is detected, sound continuous buzzer and keep gates open
    digitalWrite(buzzer, HIGH);
    return;
  } else {
    // Close gates, red LEDs on, buzzer on
    digitalWrite(greenLED1, LOW);
    digitalWrite(greenLED2, LOW);
    digitalWrite(redLED1, HIGH);
    digitalWrite(redLED2, HIGH);
    
    digitalWrite(buzzer, HIGH); // Sound buzzer
    
    leftGateServo.write(gateClosedPosition);  // Close left gate
    rightGateServo.write(gateClosedPosition); // Close right gate
  }
}

void openGates() {
  // Open gates, green LEDs on, buzzer off
  digitalWrite(redLED1, LOW);
  digitalWrite(redLED2, LOW);
  digitalWrite(greenLED1, HIGH);
  digitalWrite(greenLED2, HIGH);
  
  digitalWrite(buzzer, LOW); // Stop buzzer
  
  leftGateServo.write(gateOpenPosition);  // Open left gate
  rightGateServo.write(gateOpenPosition); // Open right gate
}