#include <Arduino.h>

#define TRIG_PIN 5
#define ECHO_PIN 18
#define LED_PIN 2

void ultrasonicTask(void *pvParameters) {

    for (;;) {

        // Trigger pulse
        digitalWrite(TRIG_PIN, LOW);
        delayMicroseconds(2);

        digitalWrite(TRIG_PIN, HIGH);
        delayMicroseconds(10);
        digitalWrite(TRIG_PIN, LOW);

        // Measure echo
        long duration = pulseIn(ECHO_PIN, HIGH, 30000);

        if (duration == 0) {
            Serial.println("Out of range");
            digitalWrite(LED_PIN, LOW);
        } 
        else {

            float distance = (duration * 0.034) / 2;

            Serial.print("Distance: ");
            Serial.print(distance);
            Serial.println(" cm");

            if (distance > 50) {

                Serial.println("LED BLINKING");

                digitalWrite(LED_PIN, HIGH);
                vTaskDelay(pdMS_TO_TICKS(300));

                digitalWrite(LED_PIN, LOW);
                vTaskDelay(pdMS_TO_TICKS(300));

            } 
            else {

                digitalWrite(LED_PIN, LOW);
                Serial.println("LED OFF");
            }
        }

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void setup() {

    Serial.begin(115200);

    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    pinMode(LED_PIN, OUTPUT);

    xTaskCreate(
        ultrasonicTask,
        "Ultrasonic_Task",
        2048,
        NULL,
        1,
        NULL
    );
}

void loop() {
}