/* Assume and four way road with traffic lights
    let
      A (Top traffic light): Controls traffic from the South road
      B (Right traffic light): Controls traffic from the West road.
      C (Bottom traffic light): Controls traffic from the North road.
      D (Left traffic light): Controls traffic from the East road.


              North Road
                  |
                  A
                  |
                  |
 West Road -- D --+-- B -- East Road
                  |
                  |
                  C
                  |
              South Road


  */

#include <LiquidCrystal_I2C.h>

/*
  The traffic array contains pins which are the pins of corresponding lights
  it contains three light the below format

  TrafficLight_Pins[]= {Red , Yellow, Green}

  used to easy accessing of corresponding traffic light
*/

const int A_Lights_Pins[] = {2, 0, 4}; 
const int B_Lights_Pins[] = {16, 17, 5}; 
const int C_Lights_Pins[] = {18, 19, 21};
const int D_Lights_Pins[] = {13, 12, 14};

// Index variable that refers to the color of the traffic light
const int Red = 0, Yellow = 1, Green = 2;

// PIR (Motion sensor) parent taffic light pins
const int A_traffic_detect = 27; 
const int B_traffic_detect = 33;
const int C_traffic_detect = 25;
const int D_traffic_detect = 26;

// Here is the structure for traffic 
struct Traffic {
    int Traffic_roads_length; // size of the traffic roads
    char Traffic_roads_queue[10]; // traffic waiting in the roads
}traffic;


// set the LCD number of columns and rows
int lcdColumns = 16;
int lcdRows = 4;
// set LCD address, number of columns and rows
LiquidCrystal_I2C Display(0x27, lcdColumns, lcdRows);  

void setup() {
    // Initialize serial communication
    Serial.begin(9600);

    // Initialize the LCD
    Display.init();  

    // Turn on the backlight
    Display.backlight();


    // Set up A_Lights_Pins as OUTPUT
    for(int i = 0; i < 3; i++) {
        pinMode(A_Lights_Pins[i], OUTPUT);
    }

    // Set up B_Lights_Pins as OUTPUT
    for(int i = 0; i < 3; i++) {
        pinMode(B_Lights_Pins[i], OUTPUT);
    }

    // Set up C_Lights_Pins as OUTPUT
    for(int i = 0; i < 3; i++) {
        pinMode(C_Lights_Pins[i], OUTPUT);
    }

    // Set up D_Lights_Pins as OUTPUT
    for(int i = 0; i < 3; i++) {
        pinMode(D_Lights_Pins[i], OUTPUT);
    }

    // Set up PIR sensors as INPUT
    pinMode(A_traffic_detect,INPUT);
    pinMode(B_traffic_detect,INPUT);
    pinMode(C_traffic_detect,INPUT);
    pinMode(D_traffic_detect,INPUT);
}

void loop() {

    Display.setCursor(2, 0);
    Display.print("Traffic System");

    // checking for traffic on roads
    const int A_traffic_result=digitalRead(A_traffic_detect);
    const int B_traffic_result=digitalRead(B_traffic_detect);
    const int C_traffic_result=digitalRead(C_traffic_detect);
    const int D_traffic_result=digitalRead(D_traffic_detect);

    if (A_traffic_result == 1)
      Serial.print("Traffic detected: A");
      Push_road(&traffic,'A');
    if (B_traffic_result == 1)
      Serial.print("Traffic detected: B");
      Push_road(&traffic,'B');
    if (C_traffic_result == 1)
      Serial.print("Traffic detected: C");
      Push_road(&traffic,'C');
    if (D_traffic_result == 1)
      Serial.print("Traffic detected: D");
      Push_road(&traffic,'D');

    if (IsTraffic(&traffic)){
      char road=Pop_road_Get(&traffic);
      Serial.print("Changing signal for road: ");
      Serial.println(road);
      Put_road_signal(road);
    }
    else
      Serial.print("No traffic detected.....");
}


// Function for make road signals
void Put_road_signal(char road) {
    if (road == 'A') {
        // Transition A lights from red to green
        digitalWrite(A_Lights_Pins[Green], HIGH);

    }
}




// Function for checking if there is traffic on any road
bool IsTraffic(struct Traffic *traffic) {
    traffic->Traffic_roads_length = sizeof(traffic->Traffic_roads_queue) / sizeof(traffic->Traffic_roads_queue[0]);
    return traffic->Traffic_roads_length > 0;
}
 
// Function to add traffic occuring road to Traffic_roads_queue
void Push_road(struct Traffic *traffic, char Traffic_occured_road) {
    if (traffic->Traffic_roads_length < 10) {
        traffic->Traffic_roads_queue[traffic->Traffic_roads_length] = Traffic_occured_road;
        traffic->Traffic_roads_length++;
    }
}

// Function get first road in the queue and poping out from // Function to add traffic occuring road to Traffic_roads_queue
char Pop_road_Get(struct Traffic *traffic) {
    char road = traffic->Traffic_roads_queue[0];
    for(int i = 0; i < traffic->Traffic_roads_length - 1; i++) {
        traffic->Traffic_roads_queue[i] = traffic->Traffic_roads_queue[i + 1];
    }
    traffic->Traffic_roads_length--;
    return road;
}