// Arduino Uno 6-Sensor Master System
// - Sensors S1 & S2: Measure object speed.
// - Sensors S3, S4, S5: Control traffic light based on zone occupancy.
// - Sensor S6: General approach detector (arms the speed trap).

#include <LiquidCrystal_I2C.h>

// --- I2C LCD SETUP ---
LiquidCrystal_I2C lcd(0x27, 16, 2); 

// --- LED PINS (Analog pins used as Digital Outputs) ---
const int LED_RED = A2; 
const int LED_YELLOW = A1; 
const int LED_GREEN = A0; 

// --- SENSOR 1 (Speed START) PINS ---
const int TRIG_PIN_1 = 2; 
const int ECHO_PIN_1 = 3; 
// --- SENSOR 2 (Speed STOP) PINS ---
const int TRIG_PIN_2 = 4; 
const int ECHO_PIN_2 = 5; 

// --- SENSOR 3 (RED Zone) PINS ---
const int TRIG_PIN_3 = 6; 
const int ECHO_PIN_3 = 7; 
// --- SENSOR 4 (YELLOW Zone) PINS ---
const int TRIG_PIN_4 = 8; 
const int ECHO_PIN_4 = 9; 
// --- SENSOR 5 (GREEN Zone) PINS ---
const int TRIG_PIN_5 = 10; 
const int ECHO_PIN_5 = 11; 

// --- SENSOR 6 (Approach/Arm) PINS ---
const int TRIG_PIN_6 = 12; 
const int ECHO_PIN_6 = 13; 

// --- SPEED & DETECTION PARAMETERS ---
// Distance between SENSOR 1 and SENSOR 2 (for speed calculation)
const float KNOWN_DISTANCE_CM = 25.0; 
const float SPEED_LIMIT_CM_S = 50.0; 

// The distance (in cm) an object must be closer than to trigger any sensor.
const int DETECTION_THRESHOLD_CM = 15; 

// Speed of sound in cm/µs
const float SOUND_SPEED = 0.0343; 

long startTime_us = 0;

/**
 * @brief Measures the distance from the sensor to the nearest object.
 * @return The calculated distance in centimeters (cm).
 */
float getDistance(int trigPin, int echoPin) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    long duration = pulseIn(echoPin, HIGH, 30000); 

    if (duration == 0) {
        return 9999.0;
    }
    
    float distance = (duration * SOUND_SPEED) / 2.0; 
    return distance;
}

/**
 * @brief Checks if an object is within the detection threshold.
 */
bool isObjectDetected(int trigPin, int echoPin) {
    return getDistance(trigPin, echoPin) < DETECTION_THRESHOLD_CM;
}

/**
 * @brief Sets the state of the traffic light LEDs.
 */
void setTrafficLight(bool red, bool yellow, bool green) {
    digitalWrite(LED_RED, red ? HIGH : LOW);
    digitalWrite(LED_YELLOW, yellow ? HIGH : LOW);
    digitalWrite(LED_GREEN, green ? HIGH : LOW);
}

/**
 * @brief Displays the current operational status based on zones.
 */
void updateTrafficStatus() {
    // Read the three zone sensors
    bool detectedS3_Red = isObjectDetected(TRIG_PIN_3, ECHO_PIN_3);
    bool detectedS4_Yellow = isObjectDetected(TRIG_PIN_4, ECHO_PIN_4);
    // bool detectedS5_Green = isObjectDetected(TRIG_PIN_5, ECHO_PIN_5); // Not strictly needed for cascading logic

    lcd.clear();
    lcd.setCursor(0, 0);

    // Cascading Priority: Red > Yellow > Green
    if (detectedS3_Red) {
        setTrafficLight(true, false, false); // RED
        lcd.print("RED ZONE: STOP!");
        Serial.println("ZONE: S3 (RED) Occupied.");
    } 
    else if (detectedS4_Yellow) {
        setTrafficLight(false, true, false); // YELLOW
        lcd.print("YELLOW ZONE: CAUTION");
        Serial.println("ZONE: S4 (YELLOW) Occupied.");
    } 
    else {
        setTrafficLight(false, false, true); // GREEN (or S5 detected)
        lcd.print("GREEN ZONE: CLEAR");
        Serial.println("ZONE: All Clear.");
    }
    lcd.setCursor(0, 1);
    lcd.print("Speed Ready");
}


/**
 * @brief Executes the S1/S2 speed measurement sequence.
 */
void executeSpeedTrap() {
    
    setTrafficLight(false, true, false); // Flash YELLOW while timing
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("SPEED TRAP ACTIVE");
    
    // --- Stage 1: START TIMER (Sensor 1) ---
    // Wait until the object triggers Sensor 1
    while (getDistance(TRIG_PIN_1, ECHO_PIN_1) >= DETECTION_THRESHOLD_CM) {
        // Debounce to ensure S1 detection is solid before starting
        if (getDistance(TRIG_PIN_6, ECHO_PIN_6) < DETECTION_THRESHOLD_CM) {
             lcd.setCursor(0, 1);
             lcd.print("S1 Waiting...");
        }
    }
    
    startTime_us = micros(); // Capture the precise start time
    lcd.setCursor(0, 1);
    lcd.print("TIMER STARTED!  ");
    Serial.println(">>> S1 (Start) Triggered. Timer START.");

    // --- Stage 2: STOP TIMER (Sensor 2) ---
    // Wait for the object to trigger Sensor 2
    while (getDistance(TRIG_PIN_2, ECHO_PIN_2) >= DETECTION_THRESHOLD_CM) {
        // Wait for detection...
    }

    long endTime_us = micros();
    long timeElapsed_us = endTime_us - startTime_us;
    Serial.println(">>> S2 (Stop) Triggered. Timer STOP.");

    // --- Calculation and Result ---
    if (timeElapsed_us > 0) {
        float timeElapsed_s = timeElapsed_us / 1000000.0; 
        float measured_speed_cms = KNOWN_DISTANCE_CM / timeElapsed_s;
        
        lcd.clear();
        lcd.setCursor(0, 0);
        
        if (measured_speed_cms > SPEED_LIMIT_CM_S) {
            setTrafficLight(true, false, false); // RED for Violation
            lcd.print("SPEED VIOLATION!");
            Serial.println("\n*** SPEED VIOLATION ***");
        } else {
            setTrafficLight(false, false, true); // GREEN for Safe
            lcd.print("Safe Speed");
        }

        lcd.setCursor(0, 1);
        lcd.print(measured_speed_cms, 2); 
        lcd.print(" cm/s");
        Serial.print("Calculated Speed: ");
        Serial.print(measured_speed_cms, 2); 
        Serial.println(" cm/s");
        Serial.println("------------------------------------");

    } else {
        // Handle error case (too fast, or error in sequence)
        setTrafficLight(true, true, false); // RED and YELLOW (Error)
        lcd.clear();
        lcd.print("Timing Error!");
        Serial.println("Error: Time measurement failed.");
    }

    // Wait 3 seconds to show the speed result before returning to traffic status
    delay(3000); 
}

void setup() {
    Serial.begin(115200); 
    
    // Initialize LCD
    lcd.init();
    lcd.backlight();
    
    // Define all 12 sensor pins (D2-D13) and 3 LED pins (A0-A2)
    pinMode(TRIG_PIN_1, OUTPUT); pinMode(ECHO_PIN_1, INPUT);
    pinMode(TRIG_PIN_2, OUTPUT); pinMode(ECHO_PIN_2, INPUT);
    pinMode(TRIG_PIN_3, OUTPUT); pinMode(ECHO_PIN_3, INPUT);
    pinMode(TRIG_PIN_4, OUTPUT); pinMode(ECHO_PIN_4, INPUT);
    pinMode(TRIG_PIN_5, OUTPUT); pinMode(ECHO_PIN_5, INPUT);
    pinMode(TRIG_PIN_6, OUTPUT); pinMode(ECHO_PIN_6, INPUT);
    
    pinMode(LED_RED, OUTPUT);
    pinMode(LED_YELLOW, OUTPUT);
    pinMode(LED_GREEN, OUTPUT);
    
    // Ensure all sensor trigger pins start LOW
    digitalWrite(TRIG_PIN_1, LOW);
    digitalWrite(TRIG_PIN_2, LOW);
    digitalWrite(TRIG_PIN_3, LOW);
    digitalWrite(TRIG_PIN_4, LOW);
    digitalWrite(TRIG_PIN_5, LOW);
    digitalWrite(TRIG_PIN_6, LOW);


    Serial.println("--- 6-Sensor Master System Initialized ---");
    updateTrafficStatus(); // Set initial status
}

void loop() {
    
    // --- System Check: Speed Trap Arming (Sensor 6) ---
    // If an object is detected by the dedicated approach sensor (S6), execute the speed sequence.
    if (isObjectDetected(TRIG_PIN_6, ECHO_PIN_6)) {
        executeSpeedTrap();
    }
    
    // --- Default State: Traffic Light Monitoring ---
    // If the speed trap is not active (S6 is clear), continuously update the traffic light based on zones S3, S4, S5.
    else {
        updateTrafficStatus();
        // Short delay to avoid excessive screen/serial updates
        delay(100); 
    }
}