#define ECHO_TOP 2
  #define TRIG_TOP 3
  #define ECHO_RIGHT 4
  #define TRIG_RIGHT 5
  #define RED_TOP 6
  #define YELLOW_TOP 7
  #define GREEN_TOP 8
  #define RED_RIGHT 9
  #define YELLOW_RIGHT 10
  #define GREEN_RIGHT 11
  #define TRAFFIC_CAM_TOP 12
  #define TRAFFIC_CAM_RIGHT 13
  #define PEDESTRIAN_TOP A0
  #define PEDESTRIAN_RIGHT A1
  //int times=1;
  String incomingDecision;

void setup() 
{
  Serial.begin(9600);
  pinMode(ECHO_TOP, INPUT);
  pinMode(TRIG_TOP, OUTPUT);
  pinMode(ECHO_RIGHT, INPUT);
  pinMode(TRIG_RIGHT, OUTPUT);
  pinMode(RED_TOP, OUTPUT);
  pinMode(YELLOW_TOP, OUTPUT);
  pinMode(GREEN_TOP, OUTPUT);
  pinMode(RED_RIGHT, OUTPUT);
  pinMode(YELLOW_RIGHT, OUTPUT);
  pinMode(GREEN_RIGHT, OUTPUT);
  pinMode(TRAFFIC_CAM_TOP, OUTPUT);
  pinMode(TRAFFIC_CAM_RIGHT, OUTPUT);
  pinMode(PEDESTRIAN_TOP, OUTPUT);
  pinMode(PEDESTRIAN_RIGHT, OUTPUT);
}

void loop() 
{
  //Print the loop iteration
  //Serial.print("Loop ");
  //Serial.println(times);
  //Serial.println("-------");

  //North South
  TrafficLight(RED_TOP, RED_RIGHT, YELLOW_TOP, GREEN_TOP, PEDESTRIAN_TOP, PEDESTRIAN_RIGHT, TRAFFIC_CAM_TOP, TRIG_TOP, ECHO_TOP, 0);

  //East West
  TrafficLight(RED_TOP, RED_RIGHT, YELLOW_RIGHT, GREEN_RIGHT, PEDESTRIAN_TOP, PEDESTRIAN_RIGHT, TRAFFIC_CAM_RIGHT, TRIG_RIGHT, ECHO_RIGHT, 1);

  //Serial.println();
  //times=times+1;
}

void TrafficLight(int red_top, int red_right, int yellow, int green, int pedestrian_top, int pedestrian_right, int traffic_cam, int trig, int echo, int direction)
{
  //Traffic Intersection makes all lights red.
  digitalWrite(red_top, HIGH);
  digitalWrite(red_right, HIGH);
  delay(500);
  //Pedestrian Lights for pedestrian crossing turn on and off.
  digitalWrite(pedestrian_top,HIGH);
  digitalWrite(pedestrian_right,HIGH);
  delay(1000);
  digitalWrite(pedestrian_top,LOW);
  digitalWrite(pedestrian_right,LOW);
  //Red Light turns off.
  delay(500);
  if(direction == 0)
  {
    digitalWrite(red_top, LOW);
  }

  else
  {
    digitalWrite(red_right, LOW);
  }
  //Green Light turns on and off.
  digitalWrite(green, HIGH);
  delay(2000);
  digitalWrite(green, LOW);
  //Yellow Light turns on and off.
  digitalWrite(yellow, HIGH);
  delay(1500);
  readSpeed(trig, echo, yellow, traffic_cam);
  delay(500);
  digitalWrite(yellow, LOW);
}

void readSpeed(int trig, int echo, int yellow, int traffic_cam)
{
  //Distance 1 trigger
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  //Distance 1 Measurement
  int duration1 = pulseIn(echo, HIGH);
  float distanceMiles1=random(3000,25000)/148;
  Serial.println(distanceMiles1,7);
  //9377280;

  //Eliminate Interference
  delay(100);

  //Distance 2 trigger
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  //Distance 2 measurement
  int duration2 = pulseIn(echo, HIGH);
  float distanceMiles2=random(3000,12000)/148;
  Serial.println(distanceMiles2,7);
  //9377280;

  //Measure speed
  float avgDistanceMiles = (distanceMiles1 - distanceMiles2)/ 5280.0;
  Serial.println(avgDistanceMiles,7);
  float timeHours = 100.0 / (1000.0 * 3600.0);
   Serial.println(timeHours,7);
  float speed=avgDistanceMiles/timeHours;
  Serial.println(speed,7);
  //Display speed
  if(yellow == 10)
  {
    //Serial.print("East and West speed: ");
    //Serial.println(speed);
  }

  else
  {
    //Serial.print("North and South speed: ");
    //Serial.println(speed);
  }
  //Read Serial
  delay(1000);
 if (Serial.available()) 
 {
    digitalWrite(traffic_cam, HIGH);
    delay(200);
    digitalWrite(traffic_cam, LOW);
    incomingDecision = Serial.read();

     //If Yellow light extension is activated
    if (incomingDecision == 1)
    {
      digitalWrite(yellow, HIGH);
      delay(900);
      digitalWrite(yellow, LOW);
      if(yellow == 10)
      {
        //Serial.println("East and West Speed Camera Triggered!");
      }

      else
      {
        //Serial.println("North and South Speed Camera Triggered!");
      }

      digitalWrite(yellow, HIGH);
      delay(900);
      digitalWrite(yellow, LOW);
      //Speed camera turned on.
      digitalWrite(traffic_cam, HIGH);
      delay(900);
      digitalWrite(traffic_cam, LOW);
    }

    else
    {
     //Serial.println("All is silent!");
    }
  }
}