#include <WiFi.h>

const int LED_PINS[3][3] = {
  {25, 26, 33},
  {19, 18, 5},
  {2, 4, 16}
};

const int SENSORS[3] = {35, 32, 34};

const int SWITCHES[3] = {15, 27, 14};

int selectedIntersection = 0; 
int currentActiveIntersection = 1;
int trafficState = 0; 
unsigned long lastChangeTime = 0;


const unsigned long GREEN_TIME_SHORT = 30000;
const unsigned long GREEN_TIME_LONG = 60000;
const unsigned long YELLOW_TIME = 5000;


const int TRAFFIC_THRESHOLD = 2048;

void setup() {
  Serial.begin(115200);
  
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++) pinMode(LED_PINS[i][j], OUTPUT);
    pinMode(SWITCHES[i], INPUT_PULLUP);
  }

  setAllRed();
  lastChangeTime = millis();
  Serial.println("Traffic Light Control System Started");
}

void loop() {
  readSwitches();

  if(selectedIntersection) {
    manualControl();
  } else {
    automaticControl();
  }

  delay(100);
}

void readSwitches() {
  static int lastSelected = 0;
  selectedIntersection = 0;

  for(int i=0; i<3; i++) {
    if(digitalRead(SWITCHES[i]) == LOW) selectedIntersection = i + 1;
  }

  if(selectedIntersection != lastSelected) {
    if(selectedIntersection) {
      Serial.print("Manual Mode - Intersection ");
      Serial.println(selectedIntersection);
    } else {
      Serial.println("Automatic Mode activated");
      setAllRed();
      currentActiveIntersection = findHighestTraffic();
      if(!currentActiveIntersection) currentActiveIntersection = 1;
      trafficState = 0;
      lastChangeTime = millis();
    }
    lastSelected = selectedIntersection;
  }
}

void manualControl() {
  for(int i=1; i<=3; i++) setIntersectionRed(i);
  if(selectedIntersection >= 1 && selectedIntersection <= 3)
    setIntersectionGreen(selectedIntersection);
}

void automaticControl() {
  unsigned long elapsed = millis() - lastChangeTime;

  switch(trafficState) {
    case 0: { 
      currentActiveIntersection = findHighestTraffic();
      if(currentActiveIntersection == 0) currentActiveIntersection = 1;

      int trafficAmount = getTrafficLevel(currentActiveIntersection);
      unsigned long greenTime = (trafficAmount < TRAFFIC_THRESHOLD/2) ? GREEN_TIME_SHORT : GREEN_TIME_LONG;

      setIntersectionGreen(currentActiveIntersection);
      trafficState = 1;
      lastChangeTime = millis();

      Serial.print("Intersection ");
      Serial.print(currentActiveIntersection);
      Serial.print(" Green for ");
      Serial.print(greenTime/1000);
      Serial.println(" seconds");
      break;
    }

    case 1: {
      int trafficAmountNow = getTrafficLevel(currentActiveIntersection);
      unsigned long greenTimeNow = (trafficAmountNow < TRAFFIC_THRESHOLD/2) ? GREEN_TIME_SHORT : GREEN_TIME_LONG;

      if(elapsed >= greenTimeNow) {
        setIntersectionYellow(currentActiveIntersection);
        trafficState = 2;
        lastChangeTime = millis();
        Serial.print("Intersection ");
        Serial.print(currentActiveIntersection);
        Serial.println(" Yellow");
      }
      break;
    }

    case 2: { 
      if(elapsed >= YELLOW_TIME) {
        setAllRed();
        trafficState = 0;
        lastChangeTime = millis();
        Serial.println("All red - preparing for next cycle");
      }
      break;
    }
  }
}

int findHighestTraffic() {
  int maxT = 0, maxI = 0;
  for(int i=0; i<3; i++){
    int t = getTrafficLevel(i+1);
    if(t > maxT){
      maxT = t;
      maxI = i + 1;
    }
  }
  if(maxT == 0) return 1;
  return maxI;
}

int getTrafficLevel(int intersection){ 
  return analogRead(SENSORS[intersection-1]); 
}

void setIntersectionGreen(int i){ 
  setIntersectionOff(i); 
  digitalWrite(LED_PINS[i-1][2], HIGH); 
  for(int j=1; j<=3; j++) if(j != i) setIntersectionRed(j); 
}

void setIntersectionYellow(int i){ 
  setIntersectionOff(i); 
  digitalWrite(LED_PINS[i-1][1], HIGH); 
}

void setIntersectionRed(int i){ 
  setIntersectionOff(i); 
  digitalWrite(LED_PINS[i-1][0], HIGH); 
}

void setIntersectionOff(int i){ 
  for(int j=0; j<3; j++) digitalWrite(LED_PINS[i-1][j], LOW);
}

void setAllRed(){ 
  for(int i=1; i<=3; i++) setIntersectionRed(i); 
}