#define BLINK_SPEED 400          // [ms] smaller number is faster blinking
#define GATE_DELAY 1500          // [ms] time between start blinking and gate closing
#define END_OF_TRAIN_DELAY 2000  // [ms] time to wait before deciding this was the end of the train
#define NUM_SENSORS 6            // two sensors per track, one left and one right of the gate

byte sensor_pin[NUM_SENSORS] = { 2, 3, 4, 5, 6, 7 };  // sensor "OUT" wires connect to these pins.
/*sensor pin colors blue, white, purple, brown, yellow, gray  */

#define LED1_PIN 8  //one of the flashing lights.
#define LED2_PIN 9  //the second flashing light.

#define Bell 13

byte train_counter, n;
byte led1, led2, blink_enabled;
byte sensor_state[NUM_SENSORS];  // 0 idle, 1 detect arrival, 2 detect departure, 3 detect end of train
byte end_of_train[NUM_SENSORS];  // 0 idle, 1 end of train detected
unsigned long time_to_blink;
unsigned long time_to_close_gate;
unsigned long time_end_of_train[NUM_SENSORS];
bool blinking_started = false; // To track if blinking has started for the current train

void setup() {
  pinMode(LED1_PIN, OUTPUT);
  pinMode(LED2_PIN, OUTPUT);
  pinMode(Bell, OUTPUT);
  for (byte i = 0; i < NUM_SENSORS; i++) pinMode(sensor_pin[i], INPUT_PULLUP);
  Serial.begin(9600);
  Serial.println("Railway Crossing Control Ready");
  Serial.println();
  Serial.println("Waiting for train");
  for (byte i = 0; i < NUM_SENSORS; i++) sensor_state[i] = 1;  // enable sensors for train detection
}

void loop() {

  for (byte i = 0; i < NUM_SENSORS; i++) {
    if (sensor_state[i] == 1) {           // detect arrival of new train
      if (!digitalRead(sensor_pin[i])) {  // train detected
        train_counter++;
        sensor_state[i] = 0;
        if (i % 2) n = i - 1;
        else n = i + 1;
        sensor_state[n] = 2;  // buddy sensor departure detection enabled
        Serial.print("Arrival:   ");
        Serial.println(i);
        Serial.print("Trains:    ");
        Serial.println(train_counter);
        
        if (!blinking_started) { // Print "Blinking started" only once per train
          Serial.println("Blinking started");
          blinking_started = true;
        }
      }
    } else if (sensor_state[i] > 1) {
      if (!digitalRead(sensor_pin[i])) {  // departure detected
        time_end_of_train[i] = millis() + (unsigned long)END_OF_TRAIN_DELAY;
        if (i % 2) n = i - 1;
        else n = i + 1;
        sensor_state[n] = 1;  // buddy sensor enabled again
        if (sensor_state[i] == 2) {
          Serial.print("Departure: ");
          Serial.println(i);
        }
        sensor_state[i] = 3;
      }
      if (sensor_state[i] == 3)  // decide if end of train has passed based on a timer
        if (millis() > time_end_of_train[i]) end_of_train[i] = 1;
      if (end_of_train[i]) {  // this takes care train_counter-- is executed only once
        train_counter--;
        end_of_train[i] = 0;
        sensor_state[i] = 1;
        Serial.print("Trains:    ");
        Serial.println(train_counter);
      }
    }
  }

  if (train_counter) {  // A train is detected.
    blink_enabled = 1;
    time_to_close_gate = millis() + (unsigned long)GATE_DELAY;
  }

  if(blink_enabled == 1) {
    if(millis() > time_to_blink) {
      time_to_blink = millis() + (unsigned long)BLINK_SPEED;
      led1 = !led1;
      led2 = !led1;
      digitalWrite(Bell, HIGH);
    }
  
    digitalWrite(LED1_PIN, led1);
    digitalWrite(LED2_PIN, led2);
    digitalWrite(Bell, HIGH);
  }

  if(!train_counter && blink_enabled) {
    digitalWrite(Bell, LOW);
    digitalWrite(LED1_PIN, LOW);
    digitalWrite(LED2_PIN, LOW);
    blink_enabled = 0;  // Stop blinking
    blinking_started = false; // Reset blinking started for the next train
    Serial.println("Stop blinking");
    Serial.println();
    Serial.println("Waiting for train");
  }
}