//Nama : Fahrul Andriansyah
//NPM : 2010501036
//Prodi : Teknik Elektro
//sistem embedded pemantauan lampu lalu lintas

#include<TimerOne.h>
// PIN LED
int signal1[] = {2, 3, 4}; //jalur 1 ( LED merah, LED kuning, LED hijau)
int signal2[] = {6, 7, 8}; //jalur 2 ( LED merah, LED kuning, LED hijau)
int signal3[] = {10, 11, 12}; //jalur 3 ( LED merah, LED kuning, LED hijau)
// DELAY
int redDelay = 10000;
int yellowDelay = 2000;
// PIN SENSOR
volatile int triggerpin1 = A0;    
volatile int echopin1 = A1;       
volatile int triggerpin2 = A2;     
volatile int echopin2 = A3;        
volatile int triggerpin3 = A4;    
volatile int echopin3 = A5;            

volatile long time;                    // Variabel untuk menyimpan waktu perjalanan
volatile int S1, S2, S3;           // Variabel untuk menyimpan jarak yang ditempuh

int t = 100;  // jarak mendeteksi kendaraan (ramai)

void setup(){
  Serial.begin(115200);
  Timer1.initialize(100000);  //pengaturan waktu
  Timer1.attachInterrupt(softInterr); //menjalankan suatu fungsi setiap kali periode pengatur waktu selesai.

  // Deklarasi pin LED
  for(int i=0; i<3; i++){
    pinMode(signal1[i], OUTPUT);
    pinMode(signal2[i], OUTPUT);
    pinMode(signal3[i], OUTPUT);
  }

  // Deklarasi pin Sensor ultrasonic 
  pinMode(triggerpin1, OUTPUT);  
  pinMode(echopin1, INPUT);      
  pinMode(triggerpin2, OUTPUT);  
  pinMode(echopin2, INPUT);
  pinMode(triggerpin3, OUTPUT);  
  pinMode(echopin3, INPUT);

}

void loop()
{
  // Jika ada kendaraan di jalur 1
  while (S1<t)
  {
    signal1Function();
    Serial.println("Jalur 1 - Ramai");
    Serial.print("jarak (cm): ");
    Serial.println(S1); 
  }
  if  (S1>t)
  {
    signal01Function();
    Serial.println("Jalur 1 - Lancar");
    Serial.print("jarak (cm): ");
    Serial.println(S1); 
  }
  // Jika ada kendaraan di jalur 2
  while (S2<t)
  {
    signal2Function();
    Serial.println("Jalur 2 - Ramai");
    Serial.print("jarak (cm): ");
    Serial.println(S2); 
  }
  if  (S2>t)
  {
    signal02Function();
    Serial.println("Jalur 2 - Lancar");
    Serial.print("jarak (cm): ");
    Serial.println(S2); 
  }
  // Jika ada kendaraan di jalur 3
  while (S3<t)
  {
    signal3Function();
    Serial.println("Jalur 3 - Ramai");
    Serial.print("jarak (cm): ");
    Serial.println(S3); 

  }
   if  (S3>t)
  {
    signal03Function();
    Serial.println("Jalur 3 - Lancar");
    Serial.print("jarak (cm): ");
    Serial.println(S3); 
  }
}

// fungsi interupsi dan akan berjalan setiap kali periode pengatur waktu selesai
void softInterr()
{
  // Membaca sensor ultrasonik 1
  digitalWrite(triggerpin1, LOW);  
  delayMicroseconds(2);
  digitalWrite(triggerpin1, HIGH); 
  delayMicroseconds(10);
  digitalWrite(triggerpin1, LOW);
  time = pulseIn(echopin1, HIGH); 
  S1= time*0.034/2;

  // Membaca sensor ultrasonik 2
  digitalWrite(triggerpin2, LOW);  
  delayMicroseconds(2);
  digitalWrite(triggerpin2, HIGH); 
  delayMicroseconds(10);
  digitalWrite(triggerpin2, LOW);
  time = pulseIn(echopin2, HIGH); 
  S2= time*0.034/2;

  // Membaca sensor ultrasonik 3
  digitalWrite(triggerpin3, LOW);  
  delayMicroseconds(2);
  digitalWrite(triggerpin3, HIGH); 
  delayMicroseconds(10);
  digitalWrite(triggerpin3, LOW);
  time = pulseIn(echopin3, HIGH); 
  S3= time*0.034/2;  
}

void signal1Function() //Jalur 1 - Ramai
{
  low();
  // LED MERAH (LOW) dan  LED HIJAU (HIGH) 5 detik
  digitalWrite(signal1[0], LOW);
  digitalWrite(signal1[2], HIGH);
  delay(redDelay);

  // jika ada kendaraan di sinyal / Sensor lain
  if(S2<t || S3<t )
  {
    // LED HIJAU (LOW) dan LED KUNING (HIGH) 2 seconds
    digitalWrite(signal1[2], LOW);
    digitalWrite(signal1[1], HIGH);
    delay(yellowDelay);
  }
}

void signal2Function()  //Jalur 2 - Ramai
{
  low();
  digitalWrite(signal2[0], LOW);
  digitalWrite(signal2[2], HIGH);
  delay(redDelay);
  
  // jika ada kendaraan di sinyal / Sensor lain
  if(S1<t || S3<t )
  {
    digitalWrite(signal2[2], LOW);
    digitalWrite(signal2[1], HIGH);
    delay(yellowDelay);   
  }
}

void signal3Function() // Jalur 3 - Ramai
{
  low();
  digitalWrite(signal3[0], LOW);
  digitalWrite(signal3[2], HIGH);
  delay(redDelay);

  // jika ada kendaraan di sinyal / Sensor lain
  if(S1<t || S2<t )
  {
    digitalWrite(signal3[2], LOW);
    digitalWrite(signal3[1], HIGH);
    delay(yellowDelay);
  }  
}

void signal01Function() //Jalur 1 - Lancar
{
  low();
  digitalWrite(signal1[0], LOW);
  digitalWrite(signal1[2], HIGH);
  delay(3000);
  digitalWrite(signal1[2], LOW);
  digitalWrite(signal1[1], HIGH);
  delay(1000);   
  digitalWrite(signal1[1], LOW);
  digitalWrite(signal1[0], HIGH);
 }

void signal02Function() //Jalur 2 - Lancar
{   
  low();
  digitalWrite(signal2[0], LOW);
  digitalWrite(signal2[2], HIGH);
  delay(3000);
  digitalWrite(signal2[2], LOW);
  digitalWrite(signal2[1], HIGH);
  delay(1000);   
  digitalWrite(signal2[1], LOW);
  digitalWrite(signal2[0], HIGH);
}

void signal03Function() //Jalur 3 - Lancar
{
  low();
  digitalWrite(signal3[0], LOW);
  digitalWrite(signal3[2], HIGH);
  delay(3000);
  digitalWrite(signal3[2], LOW);
  digitalWrite(signal3[1], HIGH);
  delay(1000);   
  digitalWrite(signal3[1], LOW);
  digitalWrite(signal3[0], HIGH);
}

// semua LED (LOW) kecuali yang MERAH.
void low()
{
  for(int i=1; i<3; i++)
  {
    digitalWrite(signal1[i], LOW);
    digitalWrite(signal2[i], LOW);
    digitalWrite(signal3[i], LOW);
  }
  for(int i=0; i<1; i++)
  {
    digitalWrite(signal1[i], HIGH);
    digitalWrite(signal2[i], HIGH);
    digitalWrite(signal3[i], HIGH);
  }
}