#include <Arduino.h>
#include <Servo.h>

// Define state constants
#define IDLE 0
#define TRAIN_APPROACH 1
#define TRAIN_CROSSING 2
#define TRAIN_EXIT 3

// Initial state
volatile int state = IDLE;  // Made volatile to ensure proper access across interrupts

// Define GPIO pins
const int S1_PIN = 8;  // GPIO pin for Sensor 1
const int S2_PIN = 9;  // GPIO pin for Sensor 2
const int S3_PIN = 13;  // GPIO pin for Sensor 3

const int GATE_SERVO1_PIN = 16;  // GPIO pin for the first gate servo
const int GATE_SERVO2_PIN = 17;  // GPIO pin for the second gate servo

// Define GPIO pins for the LEDs
const int LED1_PIN = 18;  // GPIO pin for LED 1
const int LED2_PIN = 19;  // GPIO pin for LED 2

// Servo objects
Servo gateServo1;
Servo gateServo2;

// Timing variables for LED blinking
unsigned long previousMillis = 0;
const long interval = 500;  // Interval at which to blink (in milliseconds)
bool led1State = LOW;
bool led2State = HIGH;

// Interrupt flags
volatile bool S1_triggered = false;
volatile bool S2_triggered = false;
volatile bool S3_triggered = false;

// Flags to manage gate state
bool gatesClosed = false;
bool gatesOpened = false;

void setup() {
  // Start serial communication for debugging
  Serial1.begin(115200);

  // Initialize sensors and attach interrupts
  attachInterrupt(digitalPinToInterrupt(S1_PIN), triggerS1, RISING);  // Trigger on rising edge
  attachInterrupt(digitalPinToInterrupt(S2_PIN), triggerS2, RISING);  // Trigger on rising edge
  attachInterrupt(digitalPinToInterrupt(S3_PIN), triggerS3, RISING);  // Trigger on rising edge

  // Attach the servos to the GPIO pins
  gateServo1.attach(GATE_SERVO1_PIN);
  gateServo2.attach(GATE_SERVO2_PIN);

  // Initialize the gates to open position
  openGates();

  // Initialize LEDs as output pins
  pinMode(LED1_PIN, OUTPUT);
  pinMode(LED2_PIN, OUTPUT);
  
  // Ensure LEDs are off initially
  digitalWrite(LED1_PIN, LOW);
  digitalWrite(LED2_PIN, LOW);

  Serial1.println("System initialized. Gates open.");
}

void loop() {
  unsigned long currentMillis = millis();
  
  switch(state) {
    case IDLE:
      if (S1_triggered) {
        S1_triggered = false;  // Reset the flag
        Serial1.println("Sensor 1 triggered. Train is approaching.");
        closeGates();  // Close gates as soon as S1 is triggered
        gatesClosed = true;
        blinkCrossingLights(currentMillis);  // Start blinking lights as soon as S1 is triggered
        state = TRAIN_APPROACH;
      }
      break;

    case TRAIN_APPROACH:
      blinkCrossingLights(currentMillis);
      if (S2_triggered) {
        S2_triggered = false;  // Reset the flag
        Serial1.println("Sensor 2 triggered. Train at crossing.");
        state = TRAIN_CROSSING;
      }
      break;

    case TRAIN_CROSSING:
      blinkCrossingLights(currentMillis);
      if (S3_triggered) {
        S3_triggered = false;  // Reset the flag
        Serial1.println("Sensor 3 triggered. Train has cleared crossing.");
        if (gatesClosed) {
          openGates();
          gatesOpened = true;
          gatesClosed = false;
        }
        state = TRAIN_EXIT;
      }
      break;

    case TRAIN_EXIT:
      blinkCrossingLights(currentMillis);
      // Adding a delay to ensure the train has passed
      delay(2000); // 2 seconds delay to allow the train to exit completely
      Serial1.println("Train has fully exited. Returning to IDLE.");
      turnOffLights();  // Turn off the lights when returning to IDLE
      gatesOpened = false;
      state = IDLE;
      break;
  }

  delay(100);  // Small delay to debounce sensors
}

// Servo control functions
void closeGates() {
  Serial1.println("Closing gates.");
  // Set both servos to 90 degrees (close position)
  gateServo1.write(90);
  gateServo2.write(90);
  delay(1000);  // Small delay to allow servos to reach the position
}

void openGates() {
  Serial1.println("Opening gates.");
  // Set both servos to 0 degrees (open position)
  gateServo1.write(0);
  gateServo2.write(0);
  delay(1000);  // Small delay to allow servos to reach the position
}

// Function to blink the LEDs alternately
void blinkCrossingLights(unsigned long currentMillis) {
  if (currentMillis - previousMillis >= interval) {
    // Save the last time you blinked the LEDs
    previousMillis = currentMillis;

    // Toggle the LED states
    led1State = !led1State;
    led2State = !led2State;

    // Set the LEDs according to the toggled states
    digitalWrite(LED1_PIN, led1State);
    digitalWrite(LED2_PIN, led2State);
  }
}

// Function to turn off the lights when the train has exited
void turnOffLights() {
  digitalWrite(LED1_PIN, LOW);
  digitalWrite(LED2_PIN, LOW);
  Serial1.println("All lights turned off.");
}

// Interrupt service routines (ISRs)
void triggerS1() {
  S1_triggered = true;
}

void triggerS2() {
  S2_triggered = true;
}

void triggerS3() {
  S3_triggered = true;
}