#include <TimerOne.h>
int signal1[] = {13, 12, 11};
int signal2[] = {10, 9, 8};
int redDelay = 5000;
int yellowDelay = 2000;
volatile int triggerpin1 = 4;    
volatile int echopin1 = 3;       
volatile int triggerpin2 = 2;     
volatile int echopin2 = 1;        
volatile long time;                    // Variable for storing the time traveled
volatile int S1, S2;           // Variables for storing the distance covered
int t = 5;  // distance under which it will look for vehicles.
int interruptted=0;
int normalrun=1;
int emergency=0;
int road=1;
int initial=1;
int normalloop=0;


void setup(){
  Serial.begin(115200);
  Timer1.initialize(100000);  //Begin using the timer. This function must be called first. "microseconds" is the period of time the timer takes.
  Timer1.attachInterrupt(softInterr); //Run a function each time the timer period finishes.
  // Declaring LED pins as output
  for(int i=0; i<3; i++){
    pinMode(signal1[i], OUTPUT);
    pinMode(signal2[i], OUTPUT);
      }
  // Declaring ultrasonic sensor pins as output
  
	pinMode(triggerpin1, OUTPUT);  
  pinMode(echopin1, INPUT);      
  pinMode(triggerpin2, OUTPUT);  
  pinMode(echopin2, INPUT);
  
void loop()
{
	    Serial.println("Enterrig loop function");

	if(initial==1)
	{
		digitalWrite(signal1[0], HIGH);
		digitalWrite(signal2[0], HIGH);
				initial=0;

	}
	normalloop=1;
	if(S1<t)
	{
			    Serial.println("Enterrig S1checl function");
		if(road!=1)
		{
			switchroad();
			road=1;
			signal1function();
		}
		signal1function();
		normalloop=0;
	}
	if(S2<t)
	{
			    Serial.println("Enterrig S2checl function");
		if(road!=2)
		{
			switchroad();
			road=2;
			signal2function();
		}
		signal2function();
		normalloop=0;
	}
	
	
  // If there are vehicles at signal 1
    Serial.println("Entered loop function");
	if(normalrun==1)
		{
		  Serial.println("Entered normalrun function");

			if(normalloop+1 > 1)
			{
				switchroad();
				if(road+1>2)
				{
					road=0;
				}
				road++;
				normalloop=0;
			}
			normalloop++;
			Serial.println("Crossed normalrun function");
			//deciding on which road signal to tunr on
			if(road==1)
			{
				if(S11<t1)
					{
						signal1function();
					}
				else
					{
						road++;
					}
			}
			if(road==2)
			{
				if(S22<t1)
					{
						signal2function();
					}
				else
					{
					road=1;
						signal1function();	
            					}
			}
					
		}
}

// This is interrupt function and it will run each time the timer period finishes. The timer period is set at 100 milli seconds.
void softInterr()
{
  // Reading from first ultrasonic sensor
  digitalWrite(triggerpin1, LOW);  
  delayMicroseconds(2);
  digitalWrite(triggerpin1, HIGH); 
  delayMicroseconds(10);
  digitalWrite(triggerpin1, LOW);
  time = pulseIn(echopin1, HIGH); 
  S1= time*0.034/2;
	// Reading from fifth ultrasonic sensor
	digitalWrite(triggerpin3, LOW);  
  delayMicroseconds(2);
  digitalWrite(triggerpin3, HIGH); 
  delayMicroseconds(10);
  digitalWrite(triggerpin3, LOW);
  time = pulseIn(echopin3, HIGH); 
	S11= time*0.034/2;
  	// Reading from second ultrasonic sensor
  	digitalWrite(triggerpin2, LOW);  
  delayMicroseconds(2);
  digitalWrite(triggerpin2, HIGH); 
  delayMicroseconds(10);
  digitalWrite(triggerpin2, LOW);
  time = pulseIn(echopin2, HIGH); 
  S2= time*0.034/2;
	// Reading from sixth ultrasonic sensor
  	digitalWrite(triggerpin4, LOW);  
  delayMicroseconds(2);
  digitalWrite(triggerpin4, HIGH); 
  delayMicroseconds(10);
  digitalWrite(triggerpin4, LOW);
  time = pulseIn(echopin4, HIGH); 
  S22= time*0.034/2;
  
	
  // Print distance values on serial monitor for debugging
	/*
 Serial.print("  S1: ");
  Serial.print(S1);
  Serial.print("  S2: ");
  Serial.print(S2);
  
	Serial.print("S11: ");
	Serial.print(S11);
	Serial.print(" S22: ");
	Serial.print(S22);
	
*/
}

void switchroad()
{

	if(road==1)
	{
		digitalWrite(signal1[2], LOW);
		digitalWrite(signal1[1], HIGH);
		delay(100);
		digitalWrite(signal1[1], LOW);

	}
	if(road==2)
	{
		digitalWrite(signal2[2], LOW);
		digitalWrite(signal2[1], HIGH);
		delay(200);
		digitalWrite(signal2[1], LOW);
	}
	
//	Serial.println("Turning all RED ON");
	digitalWrite(signal1[0], HIGH);
	digitalWrite(signal2[0], HIGH);
	delay(50);

}

void signal1function()
{
 // Serial.println("Entered signal1function");
  digitalWrite(signal1[0], LOW);
  digitalWrite(signal1[2], HIGH);
  delay(300);  
}

void signal2function()
{
  digitalWrite(signal2[0], LOW);
  digitalWrite(signal2[2], HIGH);	
 // Serial.println("Entered signal2function");
  delay(300);
}