#include <Servo.h>
// Train crossing ©The Geek Pub, LLC - Mike Murray 2019
// Freely distributable with attribution and link to TheGeekPub.com
 
 
//Let's define easy constants for the components so we can remember them easier
const int buttonPin = 2; // the number of the pushbutton pin
const int ledPin1L = 13; // the number of the 1st Indicator LED Left pin
const int ledPin1R = 12; // the number of the 1st Indicator LED Right pin
const int ledPin2L = 11; // the number of the 2nd Indicator LED Left pin
const int ledPin2R = 10; // the number of the 2nd Indicator LED Right pin
const int servo1Pin = 7; // the number of the 1st Crossing Guard Servo pin
const int servo2Pin = 6; // the number of the 2nd Crossong Guard Servo pin
const int buzzerPin = 5; // the number of the buzzer pin (optional)
const int TRIG = 20; //..We are setting the TRIG on the ultrasonic sensor to port 23
const int ECHO = 21;
const int GREEN_PIN = 17;
const int YELLOW_PIN = 18;
const int RED_PIN = 19;
// variables will change:
int buttonState = LOW; // variable for reading the pushbutton status
 
// create servo objects
Servo myServo1;
Servo myServo2;
 
void setup() {
  // initialize the pushbutton pin as an input:
  pinMode(buttonPin, INPUT);
 
  // initialize the LED pins as an output:
  pinMode(ledPin1L, OUTPUT);
  pinMode(ledPin1R, OUTPUT);
  pinMode(ledPin2L, OUTPUT);
  pinMode(ledPin2R, OUTPUT);

  //intalizing the Ultrasonic motion sensor
  pinMode(TRIG, OUTPUT);    // TRIG pin as output
  pinMode(ECHO, INPUT);     // ECHO pin as input
 
  // initialize the servo pins
  myServo1.attach(servo1Pin);
  myServo2.attach(servo2Pin);
 
  // set inital crossing guard positions
  CrossingGuardsUp();
 
  // initialize the buzzer pin as output:
  pinMode(buzzerPin, OUTPUT);
 
  // write to the serial console that we're up!
  Serial.begin(9600);
  Serial.println("Train Crossing Active!");
}


//define a function that is called get distance gets distance of ultrasonic wave that is sent
long getDistance()// #define a function that is called get distanc
{
  digitalWrite(TRIG, HIGH); //) #set the gpio pin to a high value or 1
  delayMicroseconds(10);//#sleeps or waits for 1 micro second
  digitalWrite(TRIG, LOW);
  unsigned long int startTime = 0;
  unsigned long int endTime = 0;
  long signal_of_utrasonic_distance_sensor;
  long distance_traveled_soundwave;
  while(digitalRead(ECHO)==LOW) //if the input ro echo is false or off
  { //get the time in seconds sinec echo, the echo is where the time starts
    startTime = micros(); //record the start time
  } //it contonously resest while echo is false
  while(digitalRead(ECHO)==HIGH)
  {
    endTime = micros(); //this lets the time since the echo 
  }//it continously resets while echo is true
  //distance in cm is equal to 
  signal_of_utrasonic_distance_sensor = endTime - startTime; //This is the total time elpased since the sound wave was outputed
  distance_traveled_soundwave = signal_of_utrasonic_distance_sensor/0.000058; //inches 0.000148 maybe 58 instead
  return distance_traveled_soundwave;
}


//This is what the output should be when the function green light is to be run 
void greenLight()
{
    digitalWrite(GREEN_PIN,  HIGH); //turn will turn on green light
    digitalWrite(YELLOW_PIN,  LOW); //turn the yellow led off
    digitalWrite(RED_PIN, LOW); //turn the red led off
    digitalWrite(buzzerPin, LOW); //turn the pizzo buzzer off
}

void yellow_light() 
{
        
    digitalWrite(GREEN_PIN, LOW);   // Turn off green light
    digitalWrite(YELLOW_PIN, HIGH);  // Turn on yellow light
    digitalWrite(RED_PIN, LOW);      // Turn off red light
    digitalWrite(buzzerPin, LOW);   // Turn off buzzer
}

void red_light() 
{
    digitalWrite(GREEN_PIN, LOW);     // Turn off green light
    digitalWrite(YELLOW_PIN, LOW);    // Turn off yellow light
    digitalWrite(RED_PIN, HIGH);       // Turn on red light
    digitalWrite(buzzerPin, HIGH);    // Turn on buzzer
}

void no_light() 
{
        // Ensure all lights and buzzer are off
    digitalWrite(GREEN_PIN, LOW);   
    digitalWrite(YELLOW_PIN, LOW);  
    digitalWrite(RED_PIN, LOW);     
    digitalWrite(buzzerPin, LOW);  
}
 
void CrossingGuardsDown(){
  // Bring the Crossing Guard arms to the down state
  myServo1.write(180);
  myServo2.write(0);
}
 
void CrossingGuardsUp(){
  // Bring the Crossing Guard arms to the up state
  myServo1.write(90);
  myServo2.write(80);
}
 
void FlashLEDs(int flashTimes, int flashDelay, bool beep) {
  // Flash the crossing guard warning indicators for xx period of time
  int ctr1 = 0;
  for (ctr1 = 0; ctr1 < flashTimes; ctr1 += 1) {
  // Outer lights on, inner lights off
  digitalWrite(ledPin1L, HIGH);
  digitalWrite(ledPin2R, HIGH);
  digitalWrite(ledPin2L, LOW);
  digitalWrite(ledPin1R, LOW);
 
  // if beep is true buzz the buzzer only on this cycle
  if (beep == true) {
    digitalWrite(buzzerPin, HIGH); 
  }
  delay(flashDelay);
  // if beep is true stop the buzzer
  if (beep == true) {
    digitalWrite(buzzerPin, LOW);
  }
  // Inner lights on, outer lights off
  digitalWrite(ledPin2L, HIGH);
  digitalWrite(ledPin1R, HIGH);
  digitalWrite(ledPin1L, LOW);
  digitalWrite(ledPin2R, LOW);
  delay(flashDelay);
  }
  // all lights off at end of sequence
  digitalWrite(ledPin1L, LOW);
  digitalWrite(ledPin2R, LOW);
  digitalWrite(ledPin2L, LOW);
  digitalWrite(ledPin1R, LOW);
}
 
void loop() 
{
  // read the state of the pushbutton value:

 
  // check if the pushbutton is pressed. If it is, the buttonState is HIGH:
  if (buttonState == HIGH) 
  {
    long distance = getDistance();
    // start the crossing guard actions
    if(distance >=90)
    {
      no_light();
    }
    else if(23 < distance <90)
    {
      greenLight();

    }
    else if(16 < distance < 22)
    {
      yellow_light();
    }
    else if(distance <=15)
    {
      red_light();
    }

    // Write console notifications
    Serial.println("Train Crossing Button Pressed!");
    
    // flash the LEDs x number of times for YY milliseconds
    FlashLEDs(7, 200, true);
 

    // lower the crossing guards
    CrossingGuardsDown();
 
    // flash the LEDs x number of times for YY milliseconds
    FlashLEDs(20, 200, true);
 
    // open the crossing guards when complete
    CrossingGuardsUp();
 
    // flash the LEDs x number of times for YY milliseconds
    FlashLEDs(5, 200, false);
 
    } else 
    {
    // literally do nothing, though you could do some stuff here if you wanted....
    // which is why I put the else here... otherwise just omit the else completely...
  }
}