#define echoPin 3         // attach pin D3 Arduino to pin Echo of HC-SR04
#define trigPin 2         // attach pin D2 Arduino to pin Trig of HC-SR04
#define DEBOUNCE_DELAY 50 // debounce delay in milliseconds
#define THRESHOLD 5       // threshold for significant distance change

// Variables
long duration;             // variable for the duration of sound wave travel
float ultrasonicDist;      // variable for the distance measurement
float lastDist = 0;        // stores the last significant distance
unsigned long lastTime = 0; // last debounce time

void setup() {
    pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
    pinMode(echoPin, INPUT);  // Sets the echoPin as an INPUT
    pinMode(5, OUTPUT);       // Green LED
    pinMode(4, OUTPUT);       // Yellow LED
    pinMode(10, OUTPUT);      // Red LED

    Serial.begin(9600); // Serial Comm is starting with 9600 baudrate speed
    Serial.println("Ultrasonic Sensor HCSR04 Test"); // print some text in Serial Monitor
    Serial.println("with Arduino UNO R3");
}

void loop() {
    ultrasonicDist = getDistance();

    // Debounce and update if the change is significant
    if (millis() - lastTime > DEBOUNCE_DELAY && abs(ultrasonicDist - lastDist) > THRESHOLD) {
        lastDist = ultrasonicDist;
        lastTime = millis();

        // Check the distance and light the corresponding LED
        if (ultrasonicDist > 60) {
            // Light green LED
            digitalWrite(5, HIGH);
            digitalWrite(4, LOW);
            digitalWrite(10, LOW);
        } else if (ultrasonicDist > 10 && ultrasonicDist <= 60) {
            // Light yellow LED
            digitalWrite(5, LOW);
            digitalWrite(4, HIGH);
            digitalWrite(10, LOW);
        } else {
            // Light red LED
            digitalWrite(5, LOW);
            digitalWrite(4, LOW);
            digitalWrite(10, HIGH);
        }
    }
}

float getDistance() {
    float distance;

    // Clears the trigPin condition
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);

    // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH);

    // Calculating the distance
    distance = duration * 0.034 / 2; // Speed of sound divided by 2 (go and back)

    // Displays the distance on the Serial Monitor
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    return distance;
}