//semaphore signals

#include <Servo.h>
Servo servo1;
int Sensor = 2;
int RevSensor = 5;
int servoPin1 = 3;
int led = 4;
int unsigned long ClearTime = 5000;
bool flag = true;

void setup() {
  servo1.attach(servoPin1);
  servo1.write(60);
  digitalWrite(led, HIGH);
  pinMode(Sensor, INPUT);
  pinMode(RevSensor, INPUT);
  pinMode(led, OUTPUT);
  Serial.begin(9600);
}

void loop() {
  if (digitalRead(RevSensor) == HIGH & flag) {
    Serial.println("Opposing Direction, No Signal");
    delay(ClearTime);
    while (digitalRead(Sensor) == HIGH &flag)
    delay(ClearTime);
  }
  if (digitalRead(Sensor) == HIGH & flag){
    Serial.println("Block Occupied, Change Signal to Red");
    delay(1000);
    for(int i = 60 ; i <= 90; i = i+1 )
    {
      servo1.write(i);
      delay(175);
    }
      servo1.write(89);
      delay(200);
      servo1.write(90);
    flag = false;   
  }    
  if (digitalRead(Sensor) == LOW & !flag){
    low_with_debounce();
  }
  
}

void low_with_debounce()
{ 
  double current_millis = millis();
  while((millis() - current_millis) < 1000)
  {
    if(digitalRead(Sensor) == HIGH)
    {
      Serial.println("Train still in block, hold signal aspect");
      return 0;
    }
  }
    Serial.println("Block Clearing, Change Signal to Yellow");
    delay(9000);
      for(int i = 90 ; i >= 75; i = i-1 )
    {
      servo1.write(i);
      delay(175);
    }
      delay(30000);
    for(int i = 75 ; i >= 60; i = i-1 )
    {
      servo1.write(i);
      delay(175);
    }
    Serial.println("Block Cleared, Change Signal to Green");
    flag = true;
}