// -------- PIN CONFIGURATION --------

// Ultrasonic TRIG pins
int trigPins[8] = {2,3,4,5,6,7,8,9};

// Ultrasonic ECHO pins
int echoPins[8] = {10,11,12,13,14,15,16,17};

// Traffic Light Pins [Road][R,Y,G]
int traffic[4][3] = {
  {18,19,20},  // Road 0
  {21,47,48},  // Road 1
  {45,0,35},  // Road 2
  {36,37,38}   // Road 3
};

// -------- SETTINGS --------
long duration;
float distance;
int threshold = 20;  // Distance in cm to detect vehicle

// -------- FUNCTIONS --------

float readUltrasonic(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH, 30000);
  distance = duration * 0.034 / 2;
  return distance;
}

int getTrafficLevel(int roadIndex) {
  int sensorA = roadIndex * 2;
  int sensorB = roadIndex * 2 + 1;

  int count = 0;

  if (readUltrasonic(trigPins[sensorA], echoPins[sensorA]) < threshold)
    count++;

  if (readUltrasonic(trigPins[sensorB], echoPins[sensorB]) < threshold)
    count++;

  return count;  // 0,1,2
}

void setAllRed() {
  for(int i=0;i<4;i++){
    digitalWrite(traffic[i][0], HIGH); // Red ON
    digitalWrite(traffic[i][1], LOW);
    digitalWrite(traffic[i][2], LOW);
  }
}

void setGreen(int road) {
  setAllRed();
  
  digitalWrite(traffic[road][0], LOW);
  digitalWrite(traffic[road][2], HIGH);
}

void setYellow(int road) {
  digitalWrite(traffic[road][2], LOW);
  digitalWrite(traffic[road][1], HIGH);
}

// -------- SETUP --------

void setup() {

  for(int i=0;i<8;i++){
    pinMode(trigPins[i], OUTPUT);
    pinMode(echoPins[i], INPUT);
  }

  for(int i=0;i<4;i++){
    for(int j=0;j<3;j++){
      pinMode(traffic[i][j], OUTPUT);
    }
  }

  Serial.begin(115200);
}

// -------- LOOP --------

void loop() {

  int trafficLevels[4];

  for(int i=0;i<4;i++){
    trafficLevels[i] = getTrafficLevel(i);
  }

  // Find road with maximum traffic
  int maxRoad = 0;
  for(int i=1;i<4;i++){
    if(trafficLevels[i] > trafficLevels[maxRoad])
      maxRoad = i;
  }

  // Adaptive green time
  int greenTime;
  if(trafficLevels[maxRoad] == 2)
    greenTime = 8000;  // HIGH traffic
  else if(trafficLevels[maxRoad] == 1)
    greenTime = 5000;  // MEDIUM
  else
    greenTime = 3000;  // LOW

  Serial.print("Giving Green to Road: ");
  Serial.println(maxRoad);

  setGreen(maxRoad);
  delay(greenTime);

  setYellow(maxRoad);
  delay(2000);

  setAllRed();
  delay(1000);
}