#include "SevSeg.h"
SevSeg sevseg;

int LEDRa = 33;
int LEDGa = 32;

int LEDRb = 23;
int LEDGb = 22;

int LEDRc = 50;
int LEDGc = 51;

int LEDRd = 42;
int LEDGd = 43;

int IRa1 = 44;
int IRa2 = 52;

int IRb1 = 49;
int IRb2 = 48;

int IRc1 = 35;
int IRc2 = 37;

int IRd1 = 47;
int IRd2 = 46;

int RoadATraffic[] = {0, 0};
int RoadBTraffic[] = {0, 0};
int RoadCTraffic[] = {0, 0};
int RoadDTraffic[] = {0, 0};

char jam[] = {'a', 'b', 'c', 'd', 'a', 'b', 'c', 'd', 'a', 'b', 'c', 'd', 'a', 'b', 'c', 'd'};

void setup() {
  pinMode (LEDRa, OUTPUT);
  pinMode (LEDGa, OUTPUT);

  pinMode (LEDRb, OUTPUT);
  pinMode (LEDGb, OUTPUT);

  pinMode (LEDRc, OUTPUT);
  pinMode (LEDGc, OUTPUT);

  pinMode (LEDRd, OUTPUT);
  pinMode (LEDGd, OUTPUT);

  pinMode (IRa1, INPUT);
  pinMode (IRa2, INPUT);

  pinMode (IRb1, INPUT);
  pinMode (IRb2, INPUT);

  pinMode (IRc1, INPUT);
  pinMode (IRc2, INPUT);

  pinMode (IRd1, INPUT);
  pinMode (IRd2, INPUT);

  byte numDigits = 2;
  byte digitPins[] = {11, 10};
  byte segmentPins[] = {9, 8, 6, 3, 4, 7, 2, 5};
  bool resistorsOnSegments = true;

  byte hardwareConfig = COMMON_CATHODE;
  sevseg.begin(hardwareConfig, numDigits, digitPins, segmentPins, resistorsOnSegments);
  sevseg.setBrightness(90);

  Serial.begin(9600); // open the serial port at 9600 bps:
}

static int counter = 5;
int jam_ind = 0;
int current_route = 0;

void loop() {
    //display();
    RoadATraffic[0] = (digitalRead (IRa1) != 1) ? ( 1 ) : ( 0 );
    RoadATraffic[1] = (digitalRead (IRa2) != 1) ? ( 1 ) : ( 0 );

    RoadBTraffic[0] = (digitalRead (IRb1) != 1) ? ( 1 ) : ( 0 );
    RoadBTraffic[1] = (digitalRead (IRb2) != 1) ? ( 1 ) : ( 0 );

    RoadCTraffic[0] = (digitalRead (IRc1) != 1) ? ( 1 ) : ( 0 );
    RoadCTraffic[1] = (digitalRead (IRc2) != 1) ? ( 1 ) : ( 0 );

    RoadDTraffic[0] = (digitalRead (IRd1) != 1) ? ( 1 ) : ( 0 );
    RoadDTraffic[1] = (digitalRead (IRd2) != 1) ? ( 1 ) : ( 0 );

    int sum1 = RoadATraffic[0] + RoadATraffic[1];
    int sum2 = RoadBTraffic[0] + RoadBTraffic[1];
    int sum3 = RoadCTraffic[0] + RoadCTraffic[1];
    int sum4 = RoadDTraffic[0] + RoadDTraffic[1];

    Serial.print(current_route);
    Serial.print('\n');
    Serial.print(jam_ind);
    Serial.print('\n');
    Serial.print('\n');

    change_route(jam[current_route]);
    display();
      
    if (sum1 >= sum2 && sum1 >= sum3 && sum1 >= sum4) {     // Road A Jam
      if (jam[current_route] == 'a' && counter == 1) {
        increase_jam_ind();
        jam[jam_ind] = 'a';
      } else if (jam[current_route] != 'a' && jam[jam_ind] != 'a') {
        increase_jam_ind();
        jam[jam_ind] = 'a';
      }
    }

    if (sum2 >= sum1 && sum2 >= sum3 && sum2 >= sum4) { // Road B Jam
      if (jam[current_route] == 'b' && counter == 1) {
        increase_jam_ind();
        jam[jam_ind] = 'b';
      } else if (jam[current_route] != 'b' && jam[jam_ind] != 'b') {
        increase_jam_ind();
        jam[jam_ind] = 'b';
      }
    }
    
    if (sum3 >= sum1 && sum3 >= sum2 && sum3 >= sum4) { // Road C Jam
      if (jam[current_route] == 'c' && counter == 1) {
        increase_jam_ind();
        jam[jam_ind] = 'c';
      } else if (jam[current_route] != 'c' && jam[jam_ind] != 'c') {
        increase_jam_ind();
        jam[jam_ind] = 'c';
      }
    }
      
    if (sum4 >= sum1 && sum4 >= sum2 && sum4 >= sum3) { // Road D Jam
      if (jam[current_route] == 'd' && counter == 1) {
        increase_jam_ind();
        jam[jam_ind] = 'd';
      } else if (jam[current_route] != 'd' && jam[jam_ind] != 'd') {
        increase_jam_ind();
        jam[jam_ind] = 'd';
      }
    }  
}

void display() {
  for (int h = 1; h<10000; h++){
    sevseg.setNumber(counter);
    sevseg.refreshDisplay();  
  }
  if (counter <= 0) {
    counter = 5;
    current_route++;
  }

  if (current_route > 15) { 
    current_route = 0;
  }
  
  counter--;
}

void change_route(char route) {
  switch (route) {
    case 'a':
      digitalWrite(LEDGa, HIGH);
      digitalWrite(LEDRa, LOW);
    
      digitalWrite(LEDGb, LOW);
      digitalWrite(LEDRb, HIGH);
    
      digitalWrite(LEDGc, LOW);
      digitalWrite(LEDRc, HIGH);
    
      digitalWrite(LEDGd, LOW);
      digitalWrite(LEDRd, HIGH);
      break;

    case 'b':
      digitalWrite(LEDGa, LOW);
      digitalWrite(LEDRa, HIGH);
    
      digitalWrite(LEDGb, HIGH);
      digitalWrite(LEDRb, LOW);
    
      digitalWrite(LEDGc, LOW);
      digitalWrite(LEDRc, HIGH);
    
      digitalWrite(LEDGd, LOW);
      digitalWrite(LEDRd, HIGH);
      break;

    case 'c':
      digitalWrite(LEDGa, LOW);
      digitalWrite(LEDRa, HIGH);
    
      digitalWrite(LEDGb, LOW);
      digitalWrite(LEDRb, HIGH);
    
      digitalWrite(LEDGc, HIGH);
      digitalWrite(LEDRc, LOW);
    
      digitalWrite(LEDGd, LOW);
      digitalWrite(LEDRd, HIGH);
      break;

    case 'd':
      digitalWrite(LEDGa, LOW);
      digitalWrite(LEDRa, HIGH);
    
      digitalWrite(LEDGb, LOW);
      digitalWrite(LEDRb, HIGH);
    
      digitalWrite(LEDGc, LOW);
      digitalWrite(LEDRc, HIGH);
    
      digitalWrite(LEDGd, HIGH);
      digitalWrite(LEDRd, LOW);
      break;
  }
}

void increase_jam_ind() {
  jam_ind++;
  if (jam_ind > 15) {
    jam_ind = 0;
  }
}