#define red1Pin       7
#define yellow1Pin    6
#define green1Pin     5
#define red2Pin       4
#define yellow2Pin    3
#define green2Pin     2

#define input1        8
#define input2        9
#define input3        10
#define input4        11

int state_input1 = 0;
int state_input2 = 0;
int state_input3 = 0;
int state_input4 = 0;

int trafficLight_1_Car_Count = 0;
int trafficLight_2_Car_Count = 0;

unsigned long currentMillis = 0;
unsigned long previousMillis1 = 0;
unsigned long previousMillis2 = 0;
unsigned long previousMillis3 = 0;
unsigned long previousMillis4 = 0;
const long interval1 = 2000;
const long interval2 = 2000;
const long interval3 = 2000;
const long interval4 = 2000;
int i = 0;
int j = 0;
enum TrafficLightState {
  RED,
  GREEN,
  YELLOW
};

TrafficLightState lightState1 = RED;
TrafficLightState lightState2 = RED;

void setup() {
  Serial.begin(9600);
  setupInputsOutputs();
  turnAllLightsRED();
}

void loop() {
  readInputs();
  if (trafficLight_1_Car_Count > trafficLight_2_Car_Count) {
    Serial.println("1 [Activated]");
    resetLEDs();
    turnAllLightsRED();
    delay(1000);
    digitalWrite(red1Pin,     0);
    digitalWrite(yellow1Pin,  0);
    digitalWrite(green1Pin,   1);
    delay(5000);
    digitalWrite(green1Pin,   0);
    digitalWrite(yellow1Pin,  1);
    delay(1000);
    turnAllLightsRED();
  }
  if (trafficLight_2_Car_Count > trafficLight_1_Car_Count) {
    Serial.println("2 [Activated]");
    resetLEDs();
    turnAllLightsRED();
    delay(1000);
    digitalWrite(red2Pin,     0);
    digitalWrite(yellow2Pin,  0);
    digitalWrite(green2Pin,   1);
    delay(5000);
    digitalWrite(green2Pin,   0);
    digitalWrite(yellow2Pin,  1);
    delay(1000);
    turnAllLightsRED();
  }
  else {
    Serial.println("Normal Mode");
    controlLights_Normally();
  }
}//end of LOOP




void trafficLightControl(int redPin, int yellowPin, int greenPin, TrafficLightState &currentState, unsigned long &previousMillis, unsigned long currentMillis, long interval) {
  switch (currentState) {
    case RED:
      digitalWrite(redPin, HIGH);
      digitalWrite(yellowPin, LOW);
      digitalWrite(greenPin, LOW);
      if (currentMillis - previousMillis >= interval) {
        currentState = GREEN;
        previousMillis = currentMillis;
      }
      if (j == 1) {
        i = i + 1;
        j = 0;
      }
      if (i == 2)i = 0;
      break;

    case GREEN:
      digitalWrite(redPin, LOW);
      digitalWrite(yellowPin, LOW);
      digitalWrite(greenPin, HIGH);
      if (currentMillis - previousMillis >= interval) {
        currentState = YELLOW;
        previousMillis = currentMillis;
      }
      break;

    case YELLOW:
      digitalWrite(redPin, LOW);
      digitalWrite(yellowPin, HIGH);
      digitalWrite(greenPin, LOW);
      if (currentMillis - previousMillis >= interval) {
        currentState = RED;
        previousMillis = currentMillis;
      }
      j = 1;
      break;
  }
}


void turnAllLightsRED() {
  digitalWrite(red1Pin,     HIGH);
  digitalWrite(yellow1Pin,  LOW);
  digitalWrite(green1Pin,   LOW);
  digitalWrite(red2Pin,     HIGH);
  digitalWrite(yellow2Pin,  LOW);
  digitalWrite(green2Pin,   LOW);
}

void setupInputsOutputs() {
  pinMode(input1,     INPUT);
  pinMode(input2,     INPUT);
  pinMode(input3,     INPUT);
  pinMode(input4,     INPUT);
  pinMode(red1Pin,    OUTPUT);
  pinMode(yellow1Pin, OUTPUT);
  pinMode(green1Pin,  OUTPUT);
  pinMode(red2Pin,    OUTPUT);
  pinMode(yellow2Pin, OUTPUT);
  pinMode(green2Pin,  OUTPUT);
}


void controlLights_Normally() {
  currentMillis = millis();
  if (i == 0) {
    // Traffic Light 1
    trafficLightControl(red1Pin, yellow1Pin, green1Pin, lightState1, previousMillis1, currentMillis, interval1);
  }
  else if (i == 1) {
    // Traffic Light 2
    trafficLightControl(red2Pin, yellow2Pin, green2Pin, lightState2, previousMillis2, currentMillis, interval2);
  }
}

void readInputs() {
  state_input1 = !digitalRead(input1);
  state_input2 = !digitalRead(input2);
  state_input3 = !digitalRead(input3);
  state_input4 = !digitalRead(input4);
  trafficLight_1_Car_Count = state_input1 + state_input2;
  trafficLight_2_Car_Count = state_input3 + state_input4;

  Serial.print(state_input1);
  Serial.print(" ");
  Serial.print(state_input2);
  Serial.print(" ");
  Serial.print(state_input3);
  Serial.print(" ");
  Serial.println(state_input4);
  Serial.print(trafficLight_1_Car_Count);
  Serial.print(" ");
  Serial.println(trafficLight_2_Car_Count);

}

void resetLEDs() {
  i = 0;
  j = 0;
  lightState1 = RED;
  lightState2 = RED;
}