// GPIO pins for each traffic signal light (red, yellow, green)
int signal1[] = {26, 25, 33};
//int signal2[] = {19, 18, 5};
//int signal3[] = {32, 3, 21};
//int signal4[] = {17, 16, 15};

// Timing delays for light changes (milliseconds)
int redDelay = 5000;
int yellowDelay = 2000;

// Ultrasonic sensor pins
int triggerpin1 = 13;
int echopin1 = 12;
//int triggerpin2 = 27;
//int echopin2 = 14;
//int triggerpin3 = 22;
//int echopin3 = 23;
//int triggerpin4 = 2;
//int echopin4 = 4;

long duration; // Time for ultrasonic signal to return
int S1 ;//S2, S3, S4; // Distance measurements from each sensor

int t = 5; // Threshold distance to check for traffic presence

void setup()
{
  Serial.begin(115200);

  // Initialize traffic signal LEDs as outputs
  for (int i = 0; i < 3; i++)
  {
    pinMode(signal1[i], OUTPUT);
    /*pinMode(signal2[i], OUTPUT);
    pinMode(signal3[i], OUTPUT);
    pinMode(signal4[i], OUTPUT);*/
  }

  // Initialize ultrasonic sensor pins
  pinMode(triggerpin1, OUTPUT);
  pinMode(echopin1, INPUT);
  /*pinMode(triggerpin2, OUTPUT);
  pinMode(echopin2, INPUT);
  pinMode(triggerpin3, OUTPUT);
  pinMode(echopin3, INPUT);
  pinMode(triggerpin4, OUTPUT);
  pinMode(echopin4, INPUT);*/
}

void loop()
{
  // Measure distance from each ultrasonic sensor
  S1 = readDistance(triggerpin1, echopin1);
 /* S2 = readDistance(triggerpin2, echopin2);
  S3 = readDistance(triggerpin3, echopin3);
  S4 = readDistance(triggerpin4, echopin4);*/

  // Check and update the status of each traffic signal
  if (S1 < t)
  {
    controlSignal(signal1);
  }
  /*else if (S2 < t)
  {
    controlSignal(signal2);
  }
  else if (S3 < t)
  {
    controlSignal(signal3);
  }
  else if (S4 < t)
  {
    controlSignal(signal4);
  }*/
  else 
  {
    Serial.print("distance not correct");
  }

  // Wait a bit before next loop iteration
  delay(1000);
}

int readDistance(int triggerPin, int echoPin)
{
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2; // Convert to cm
}

// Controls the signal light changes
void controlSignal(int signalPins[])
{
  lowAll(); // Reset all signals to red before changing the current one
  digitalWrite(signalPins[0], HIGH); // Turn on red LED
  Serial.print("RED");
  delay(redDelay);

  digitalWrite(signalPins[0], LOW); // Turn off red LED

  digitalWrite(signalPins[2], HIGH); // Turn on green LED
  Serial.print("green");
  delay(redDelay);

  digitalWrite(signalPins[2], LOW); // Turn off green LED

  digitalWrite(signalPins[1], HIGH); // Turn on yellow LED
  Serial.print("yellow");
  delay(yellowDelay);

  digitalWrite(signalPins[1], LOW); // Turn off yellow LED
}

// Set all signals to red
void lowAll()
{
  for (int i = 0; i < 3; i++)
  {
    digitalWrite(signal1[i], LOW);
    /*digitalWrite(signal2[i], LOW);
    digitalWrite(signal3[i], LOW);
    digitalWrite(signal4[i], LOW);*/
  }
}