#define TRIG1_PIN 34
#define ECHO1_PIN 35
#define TRIG2_PIN 32
#define ECHO2_PIN 33
#define PIR_PIN 36
#define LDR_PIN 5
#define LIGHT1_PIN 39 // Main road light
#define TRIG3_PIN 17
#define ECHO3_PIN 16
#define TRIG4_PIN 4
#define ECHO4_PIN 0
#define LIGHT2_PIN 2

// Traffic light pins for North-South direction
#define RED_NS_PIN 25
#define YELLOW_NS_PIN 26
#define GREEN_NS_PIN 27

// Traffic light pins for East-West direction
#define RED_EW_PIN 14
#define YELLOW_EW_PIN 12
#define GREEN_EW_PIN 13

const float sensorDistance = 24.0; // Distance between two sensors in inches
const float speedConversion = 0.0568182; // Conversion factor for inches/sec to mph
const int ldrThreshold = 500; // Threshold for low light (adjust as needed)

unsigned long time1 = 0, time2 = 0, time3 = 0, time4 = 0; // Time of vehicle detection at each sensor
bool vehicleDetectedSensor1 = false, vehicleDetectedSensor2 = false, vehicleDetectedSensor3 = false, vehicleDetectedSensor4 = false; // Flags for vehicle detection
float previousDistance1 = 1000; // Initial large value to track previous distance
float previousDistance2 = 1000;
float previousDistance3 = 1000;
float previousDistance4 = 1000;

byte PWM = 255;

enum TrafficLightState { RED_NS, GREEN_NS, YELLOW_NS, RED_EW, GREEN_EW, YELLOW_EW }; // Enum for 4-way traffic light control
TrafficLightState trafficLightState = RED_NS;

unsigned long previousMillis = 0; // To track time for traffic light changes
const unsigned long redDuration = 5000; // 5 seconds red
const unsigned long yellowDuration = 2000; // 2 seconds yellow
const unsigned long greenDuration = 5000; // 5 seconds green

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

  pinMode(TRIG1_PIN, OUTPUT);
  pinMode(ECHO1_PIN, INPUT);
  pinMode(TRIG2_PIN, OUTPUT);
  pinMode(ECHO2_PIN, INPUT);
  pinMode(TRIG3_PIN, OUTPUT);
  pinMode(ECHO3_PIN, INPUT);
  pinMode(TRIG4_PIN, OUTPUT);
  pinMode(ECHO4_PIN, INPUT);
  pinMode(PIR_PIN, INPUT);
  pinMode(LIGHT1_PIN, OUTPUT);
  pinMode(LIGHT2_PIN, OUTPUT);

  // Set up North-South traffic lights
  pinMode(RED_NS_PIN, OUTPUT);
  pinMode(YELLOW_NS_PIN, OUTPUT);
  pinMode(GREEN_NS_PIN, OUTPUT);

  // Set up East-West traffic lights
  pinMode(RED_EW_PIN, OUTPUT);
  pinMode(YELLOW_EW_PIN, OUTPUT);
  pinMode(GREEN_EW_PIN, OUTPUT);

  // Start with North-South red, East-West green
  digitalWrite(RED_NS_PIN, HIGH);
  digitalWrite(YELLOW_NS_PIN, LOW);
  digitalWrite(GREEN_NS_PIN, LOW);

  digitalWrite(RED_EW_PIN, LOW);
  digitalWrite(YELLOW_EW_PIN, LOW);
  digitalWrite(GREEN_EW_PIN, HIGH);
}

void loop() {
  float distance1 = measureDistance(TRIG1_PIN, ECHO1_PIN);
  float distance2 = measureDistance(TRIG2_PIN, ECHO2_PIN);
  float distance3 = measureDistance(TRIG3_PIN, ECHO3_PIN);
  float distance4 = measureDistance(TRIG4_PIN, ECHO4_PIN);
  int ldrValue = analogRead(LDR_PIN);
  bool pedestrianDetected = digitalRead(PIR_PIN);

  // Handle traffic light state changes using millis()
  controlTrafficLights();

  // Detect vehicle at Sensor 1: Transition from >70 inches to <70 inches
  if (!vehicleDetectedSensor1 && previousDistance1 > 70 && distance1 < 70) {
    vehicleDetectedSensor1 = true;
    time1 = millis();
    Serial.println("Vehicle detected at Sensor 1");
  }
  if (vehicleDetectedSensor1 && distance1 > 70) {
    vehicleDetectedSensor1 = false; // Reset flag when vehicle leaves sensor 1
  }

  // Detect vehicle at Sensor 2: Transition from >70 inches to <70 inches
  if (!vehicleDetectedSensor2 && previousDistance2 > 70 && distance2 < 70) {
    vehicleDetectedSensor2 = true;
    time2 = millis();
    Serial.println("Vehicle detected at Sensor 2");
  }
  if (vehicleDetectedSensor2 && distance2 > 70) {
    vehicleDetectedSensor2 = false; // Reset flag when vehicle leaves sensor 2
  }
  
  // Detect vehicle at Sensor 3: Transition from >70 inches to <70 inches
  if (!vehicleDetectedSensor3 && previousDistance3 > 70 && distance3 < 70) {
    vehicleDetectedSensor3 = true;
    time3 = millis();
    Serial.println("Vehicle detected at Sensor 3");
  }
  if (vehicleDetectedSensor3 && distance3 > 70) {
    vehicleDetectedSensor3 = false; // Reset flag when vehicle leaves sensor 2
  }
  
  // Detect vehicle at Sensor 4: Transition from >70 inches to <70 inches
  if (!vehicleDetectedSensor4 && previousDistance4 > 70 && distance4 < 70) {
    vehicleDetectedSensor4 = true;
    time4 = millis();
    Serial.println("Vehicle detected at Sensor 4");
  }
  if (vehicleDetectedSensor4 && distance4 > 70) {
    vehicleDetectedSensor4 = false; // Reset flag when vehicle leaves sensor 2
  }

  // Calculate speed when both detections are made
  if (time1 > 0 && time2 > time1) {
    unsigned long timeDifference = time2 - time1; // Time difference in milliseconds
    float speedInchesPerSec = sensorDistance / (timeDifference / 1000.0); // Speed in inches/second
    float speedMph = speedInchesPerSec * speedConversion; // Convert to mph

    Serial.print("Speed: ");
    Serial.print(speedMph);
    Serial.println(" mph");

    // Indicate speed detection with LED
    analogWrite(LIGHT1_PIN, PWM);
    delay(2000); // Keep LED on for 2 seconds (blocking only for visual)
    digitalWrite(LIGHT1_PIN, LOW);

    // Reset times for next vehicle detection
    time1 = 0;
    time2 = 0;
  }
  
  if (time3 > 0 && time4 > time3) {
    unsigned long timeDifference = time4 - time3; // Time difference in milliseconds
    float speedInchesPerSec = sensorDistance / (timeDifference / 1000.0); // Speed in inches/second
    float speedMph = speedInchesPerSec * speedConversion; // Convert to mph

    Serial.print("Speed: ");
    Serial.print(speedMph);
    Serial.println(" mph");

    // Indicate speed detection with LED
    analogWrite(LIGHT2_PIN, PWM);
    delay(2000); // Keep LED on for 2 seconds (blocking only for visual)
    digitalWrite(LIGHT2_PIN, LOW);

    // Reset times for next vehicle detection
    time3 = 0;
    time4 = 0;
  }

  // LDR check for ambient light
  if (ldrValue < ldrThreshold) {
    Serial.println("Low ambient light detected.");
    PWM = 255;
  } else {
    Serial.println("Sufficient ambient light detected.");
    PWM = 0;
  }

  // PIR check for pedestrian presence
  if (pedestrianDetected) {
    Serial.println("Pedestrian detected!");
    // Optionally turn on LED for pedestrian presence
    digitalWrite(LIGHT1_PIN, PWM);
    delay(500); // Keep LED on briefly
    digitalWrite(LIGHT1_PIN, LOW);
  }

  // Store current distances for next iteration
  previousDistance1 = distance1;
  previousDistance2 = distance2;

  delay(100); // Small delay to avoid excessive sensor noise
}

// Function to measure distance using ultrasonic sensor
float measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  float distance = (duration * 0.0343) / 2; // Convert duration to distance in inches
  return distance;
}

// Non-blocking 4-way traffic light control using millis()
void controlTrafficLights() {
  unsigned long currentMillis = millis(); // Get current time

  switch (trafficLightState) {
    case RED_NS:
      if (currentMillis - previousMillis >= redDuration) {
        // Change North-South to green, East-West to red
        trafficLightState = GREEN_NS;
        previousMillis = currentMillis;
        digitalWrite(RED_NS_PIN, LOW);
        digitalWrite(GREEN_NS_PIN, HIGH);
        digitalWrite(RED_EW_PIN, HIGH);
        digitalWrite(GREEN_EW_PIN, LOW);
      }
      break;

    case GREEN_NS:
      if (currentMillis - previousMillis >= greenDuration) {
        // Change North-South to yellow
        trafficLightState = YELLOW_NS;
        previousMillis = currentMillis;
        digitalWrite(GREEN_NS_PIN, LOW);
        digitalWrite(YELLOW_NS_PIN, HIGH);
      }
      break;

    case YELLOW_NS:
      if (currentMillis - previousMillis >= yellowDuration) {
        // Change East-West to green, North-South to red
        trafficLightState = GREEN_EW;
        previousMillis = currentMillis;
        digitalWrite(YELLOW_NS_PIN, LOW);
        digitalWrite(RED_NS_PIN, HIGH);
        digitalWrite(RED_EW_PIN, LOW);
        digitalWrite(GREEN_EW_PIN, HIGH);
      }
      break;

    case GREEN_EW:
      if (currentMillis - previousMillis >= greenDuration) {
        // Change East-West to yellow
        trafficLightState = YELLOW_EW;
        previousMillis = currentMillis;
        digitalWrite(GREEN_EW_PIN, LOW);
        digitalWrite(YELLOW_EW_PIN, HIGH);
      }
      break;

    case YELLOW_EW:
      if (currentMillis - previousMillis >= yellowDuration) {
        // Change North-South to green, East-West to red
        trafficLightState = RED_NS;
        previousMillis = currentMillis;
        digitalWrite(YELLOW_EW_PIN, LOW);
        digitalWrite(RED_EW_PIN, HIGH);
        digitalWrite(GREEN_NS_PIN, HIGH);
        digitalWrite(RED_NS_PIN, LOW);
      }
      break;
  }
}