// ESP32 Dual Sensor Speed Trap (HC-SR04)
// This code measures the time taken for an object to travel a fixed distance 
// between two sensors, then calculates the speed.

// --- SENSOR 1 (START TIMER) PINS ---
// TRIG 1: GPIO 26 (Output)
const int TRIG_PIN_1 = 25; 
// ECHO 1: GPIO 27 (Input) - !!! CRITICAL: Requires a 5V-to-3.3V Voltage Divider !!!
const int ECHO_PIN_1 = 26; 

// --- SENSOR 2 (STOP TIMER) PINS ---
// TRIG 2: GPIO 18 (Output)
const int TRIG_PIN_2 = 27; 
// ECHO 2: GPIO 19 (Input) - !!! CRITICAL: Requires a 5V-to-3.3V Voltage Divider !!!
const int ECHO_PIN_2 = 14; 

// --- SPEED TRAP PARAMETERS ---

// !!! IMPORTANT: MEASURE THIS DISTANCE ACCURATELY (in centimeters) !!!
// This is the fixed distance between the face of Sensor 1 and the face of Sensor 2.
const float KNOWN_DISTANCE_CM = 25.0; // <-- CHANGE THIS VALUE!

// The max distance (in cm) an object can be from a sensor to be considered 'detected'.
// This is your trip-wire threshold.
const int DETECTION_THRESHOLD_CM = 15; 

// Speed of sound at standard temperature in cm/µs (343 m/s)
const float SOUND_SPEED = 0.0343; 

long startTime_us = 0;

/**
 * @brief Measures the distance from the sensor to the nearest object.
 * @param trigPin The GPIO pin connected to the sensor's TRIG pin.
 * @param echoPin The GPIO pin connected to the sensor's ECHO pin.
 * @return The calculated distance in centimeters (cm).
 */
float getDistance(int trigPin, int echoPin) {
    // 1. Clear the trigger pin to ensure a clean start
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    
    // 2. Send 10µs pulse to trigger the sensor
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    // 3. Measure the duration of the return echo pulse (HIGH signal time)
    // The timeout ensures the program doesn't hang if no echo is received.
    long duration = pulseIn(echoPin, HIGH, 30000); // 30ms timeout

    if (duration == 0) {
        return 9999.0; // Return a large value if timeout occurs (out of range)
    }
    
    // 4. Calculate distance: Distance = (Duration * Speed of Sound) / 2
    // We divide by 2 because the duration includes travel time both out and back.
    float distance = (duration * SOUND_SPEED) / 2.0; 
    return distance;
}

void setup() {
    // Start serial communication for debugging and output
    Serial.begin(115200); 
    
    // Define pin modes
    pinMode(TRIG_PIN_1, OUTPUT); 
    pinMode(ECHO_PIN_1, INPUT);
    pinMode(TRIG_PIN_2, OUTPUT); 
    pinMode(ECHO_PIN_2, INPUT);
    
    // Ensure trigger pins start LOW
    digitalWrite(TRIG_PIN_1, LOW);
    digitalWrite(TRIG_PIN_2, LOW);
    
    Serial.println("--- ESP32 Dual Sensor Speed Trap Initialized ---");
    Serial.print("Fixed Separation Distance: ");
    Serial.print(KNOWN_DISTANCE_CM);
    Serial.println(" cm");
    Serial.println("Waiting for object to enter trap...");
}

void loop() {
    // Stage 1: Check if object is close enough to trigger Sensor 1
    if (getDistance(TRIG_PIN_1, ECHO_PIN_1) < DETECTION_THRESHOLD_CM) {
        
        startTime_us = micros(); // Capture the precise start time (in microseconds)
        Serial.println(">>> Sensor 1 Triggered. Timer START.");

        // Stage 2: Wait for the SAME object to trigger Sensor 2 (Stop the timer)
        // This loop holds execution until Sensor 2 detects the object.
        while (getDistance(TRIG_PIN_2, ECHO_PIN_2) >= DETECTION_THRESHOLD_CM) {
            // Wait for detection...
        }

        // --- STOP TIMER ---
        long endTime_us = micros();
        long timeElapsed_us = endTime_us - startTime_us;

        if (timeElapsed_us > 0) {
            // Convert elapsed time from microseconds to seconds
            float timeElapsed_s = timeElapsed_us / 1000000.0; 
            
            // Calculate speed: Speed (cm/s) = Distance (cm) / Time (s)
            float measured_speed_cms = KNOWN_DISTANCE_CM / timeElapsed_s;

            Serial.print("Time Elapsed: ");
            Serial.print(timeElapsed_s, 6);
            Serial.println(" seconds");

            Serial.print("====================================");
            Serial.print("\n**CALCULATED SPEED: ");
            Serial.print(measured_speed_cms, 2); 
            Serial.println(" cm/s**");
            Serial.print("====================================\n");
        } else {
            Serial.println("Error: Time measurement failed (Object too fast or slow).");
        }

        // Delay to allow the object to clear the trap before re-arming
        delay(1000); 
    }
}