#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "esp_system.h"
#include "esp_rom_sys.h"
#include "esp_timer.h"

// Define GPIO pins for Trigger, Echo, and LED
#define TRIGGER_PIN 4
#define ECHO_PIN 5
#define LED_PIN 2

// Define a Queue to send distance data from ultrasonic task to LED task
QueueHandle_t distance_queue;


long read_distance() {
    gpio_set_level(TRIGGER_PIN, 1);
    esp_rom_delay_us(10);
    gpio_set_level(TRIGGER_PIN, 0);

    while (gpio_get_level(ECHO_PIN) == 0);

    long start_time = esp_timer_get_time();

    while (gpio_get_level(ECHO_PIN) == 1);

    long end_time = esp_timer_get_time();

    long duration = end_time - start_time;

    long distance = (duration / 2) / 29.1; 
    return distance;
}

// FreeRTOS task for ultrasonic sensor
void ultrasonic_task(void *pvParameter) {
    while (1) {
        long distance = read_distance();
        printf("Distance: %ld cm\n", distance);

        // Send distance value to LED task via the queue
        xQueueSend(distance_queue, &distance, portMAX_DELAY);

        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

void blink_led(void *pvParameter){
  while(1){

  }
}

// FreeRTOS task for controlling the LED
void led_task(void *pvParameter) {
    gpio_set_level(LED_PIN, 0);
    long distance;
    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);

    while (1) {
        // Receive the distance data from the queue
        if (xQueueReceive(distance_queue, &distance, portMAX_DELAY)) {
            // Check if distance is less than 10 cm, then turn on LED
            if (distance < 100) {
                gpio_set_level(LED_PIN, 1);
                vTaskDelay(pdMS_TO_TICKS(500));
                gpio_set_level(LED_PIN, 0);
                vTaskDelay(pdMS_TO_TICKS(500));
            } else {
                gpio_set_level(LED_PIN, 0);  // Turn off LED
            }
        }
    }
}

void app_main() {
    // Set GPIO pins for Trigger and Echo
    gpio_set_direction(TRIGGER_PIN, GPIO_MODE_OUTPUT);
    gpio_set_direction(ECHO_PIN, GPIO_MODE_INPUT);

    // Create the queue to pass distance data
    distance_queue = xQueueCreate(10, sizeof(long));

    if (distance_queue == NULL) {
        printf("Failed to create the queue\n");
        return;
    }

    // Create FreeRTOS tasks
    xTaskCreate(&ultrasonic_task, "Ultrasonic Task", 2048, NULL, 5, NULL);
    xTaskCreate(&led_task, "LED Task", 2048, NULL, 1, NULL);
}