#include <Servo.h> // Import Servo library

// Declare servo motors
Servo carBarrierServo1;        // First servo for car barrier
Servo carBarrierServo2;        // Second servo for car barrier
Servo pedestrianBarrierServo;  // Servo for pedestrian barrier

// Declare LEDs for traffic lights
const int redLED = 13;           // Red LED for car traffic light
const int yellowLED = 11;        // Yellow LED for car traffic light
const int greenLED = 9;          // Green LED for car traffic light
const int redPedestrianLED = 6;  // Red LED for pedestrian light
const int greenPedestrianLED = 5; // Green LED for pedestrian light

// Declare servo motor pins
const int carBarrierServoPin1 = 3; // First car barrier servo pin
const int carBarrierServoPin2 = 4; // Second car barrier servo pin
const int pedestrianBarrierServoPin = 10; // Pedestrian barrier servo pin

// Time intervals in milliseconds
const long redTime = 10000;    // 10 seconds for red light (Pedestrians cross)
const long greenTime = 10000;  // 10 seconds for green light (Cars go)
const long yellowTime = 5000;  // 5 seconds for yellow light

unsigned long previousMillis = 0;  // Store last time traffic light changed
int trafficLightState = 0;  // 0 = Red, 1 = Green, 2 = Yellow

void setup() {
  // Set up LED pins as OUTPUT
  pinMode(redLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);
  pinMode(greenLED, OUTPUT);
  pinMode(redPedestrianLED, OUTPUT);
  pinMode(greenPedestrianLED, OUTPUT);

  // Attach servo motors
  carBarrierServo1.attach(carBarrierServoPin1);
  carBarrierServo2.attach(carBarrierServoPin2);
  pedestrianBarrierServo.attach(pedestrianBarrierServoPin);

  // Start in Red State (Pedestrians Cross)
  setRedState();
}

void loop() {
  unsigned long currentMillis = millis();

  if (trafficLightState == 0 && currentMillis - previousMillis >= redTime) {
    setYellowBeforeGreen(); // Change to Yellow before Green
  } 
  else if (trafficLightState == 3 && currentMillis - previousMillis >= yellowTime) {
    setGreenState(); // Change to Green
  }
  else if (trafficLightState == 1 && currentMillis - previousMillis >= greenTime) {
    setYellowState(); // Change to Yellow before Red
  } 
  else if (trafficLightState == 2 && currentMillis - previousMillis >= yellowTime) {
    setRedState(); // Change to Red
  }
}

// 🚦 Set Traffic Light to YELLOW Before GREEN
void setYellowBeforeGreen() {
  trafficLightState = 3;
  previousMillis = millis();

  // Set traffic lights
  digitalWrite(redLED, LOW);
  digitalWrite(yellowLED, HIGH);
  digitalWrite(greenLED, LOW);

  // Keep pedestrian light red
  digitalWrite(greenPedestrianLED, LOW);
  digitalWrite(redPedestrianLED, HIGH);

  // Close pedestrian barrier
  pedestrianBarrierServo.write(90);

  // Keep car barriers closed
  carBarrierServo1.write(90);
  carBarrierServo2.write(90);
}


// ====== TRAFFIC LIGHT STATES ======

// 🚦 Set Traffic Light to RED (Cars stop, Pedestrians cross)
void setRedState() {
  trafficLightState = 0;
  previousMillis = millis();

  // Set traffic lights
  digitalWrite(redLED, HIGH);
  digitalWrite(yellowLED, LOW);
  digitalWrite(greenLED, LOW);

  // Set pedestrian lights
  digitalWrite(greenPedestrianLED, HIGH);
  digitalWrite(redPedestrianLED, LOW);

  // Close car barriers (both servos)
  carBarrierServo1.write(90);
  carBarrierServo2.write(90);

  // Open pedestrian barrier
  pedestrianBarrierServo.write(0);
}

// 🚦 Set Traffic Light to GREEN (Cars go, Pedestrians stop)
void setGreenState() {
  trafficLightState = 1;
  previousMillis = millis();

  // Set traffic lights
  digitalWrite(redLED, LOW);
  digitalWrite(yellowLED, LOW);
  digitalWrite(greenLED, HIGH);

  // Set pedestrian lights
  digitalWrite(greenPedestrianLED, LOW);
  digitalWrite(redPedestrianLED, HIGH);

  // Open car barriers (both servos)
  carBarrierServo1.write(0);
  carBarrierServo2.write(0);

  // Close pedestrian barrier
  pedestrianBarrierServo.write(90);
}

// 🚦 Set Traffic Light to YELLOW (Warning before RED)
void setYellowState() {
  trafficLightState = 2;
  previousMillis = millis();

  // Set traffic lights
  digitalWrite(redLED, LOW);
  digitalWrite(yellowLED, HIGH);
  digitalWrite(greenLED, LOW);

  // Keep pedestrian light red
  digitalWrite(greenPedestrianLED, LOW);
  digitalWrite(redPedestrianLED, HIGH);

  // Keep car barriers open (cars are still moving)
  carBarrierServo1.write(0);
  carBarrierServo2.write(0);

  // Keep pedestrian barrier closed
  pedestrianBarrierServo.write(90);
}