// Define the pins for Sensor 1
const int TRIG_PIN_1 = 25;  // GPIO 5
const int ECHO_PIN_1 = 26; // GPIO 18

// Define the pins for Sensor 2
const int TRIG_PIN_2 = 27;  // GPIO 7 (Assuming you use this pin)
const int ECHO_PIN_2 = 14; // GPIO 19 (Assuming you use this pin)

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

// --- Function to measure distance ---
float getDistance(int trigPin, int echoPin) {
    // Clear the trigger pin (ensure a low pulse before starting)
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2); 

    // Send a 10µs pulse to trigger the sensor
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    // Measure the duration of the echo pulse (in microseconds)
    // NOTE: For ESP32, pulseIn is fast, but using an interrupt-based library (like NewPing) 
    // is often better for parallel measurements. We use pulseIn for simplicity here.
    long duration = pulseIn(echoPin, HIGH, 30000); // 30ms timeout

    if (duration == 0) {
        // If pulseIn timed out (no object/bad reading)
        return -1.0; 
    }

    // Calculate distance (in cm)
    // Distance = (Duration * Speed of Sound) / 2
    float distance = (duration * SOUND_SPEED) / 2.0; 

    return distance;
}

void setup() {
    Serial.begin(115200); // Use a faster baud rate for ESP32
    
    // Setup pins for Sensor 1
    pinMode(TRIG_PIN_1, OUTPUT);
    pinMode(ECHO_PIN_1, INPUT);

    // Setup pins for Sensor 2
    pinMode(TRIG_PIN_2, OUTPUT);
    pinMode(ECHO_PIN_2, INPUT);
    
    // Initialize pins to LOW to be safe
    digitalWrite(TRIG_PIN_1, LOW);
    digitalWrite(TRIG_PIN_2, LOW);
    
    Serial.println("--- Dual Ultrasonic Sensor Test Started ---");
}

void loop() {
    // Read distances from both sensors
    float dist1 = getDistance(TRIG_PIN_1, ECHO_PIN_1);
    float dist2 = getDistance(TRIG_PIN_2, ECHO_PIN_2);

    // Display the results
    Serial.print("Sensor 1 Distance (cm): ");
    if (dist1 > 0) {
        Serial.print(dist1);
    } else {
        Serial.print("---");
    }

    Serial.print(" | Sensor 2 Distance (cm): ");
    if (dist2 > 0) {
        Serial.println(dist2);
    } else {
        Serial.println("---");
    }
    
    // Short delay for stability and readability
    delay(500); 
}