#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include "esp_rom_sys.h"  // For esp_rom_delay_us()

#define TRIG_PIN GPIO_NUM_5
#define ECHO_PIN GPIO_NUM_18

void app_main(void)
{
    gpio_set_direction(TRIG_PIN, GPIO_MODE_OUTPUT);
    gpio_set_direction(ECHO_PIN, GPIO_MODE_INPUT);

    while (1)
    {
        // Send 10us HIGH pulse on TRIG pin
        gpio_set_level(TRIG_PIN, 0);
        esp_rom_delay_us(2);
        gpio_set_level(TRIG_PIN, 1);
        esp_rom_delay_us(10);
        gpio_set_level(TRIG_PIN, 0);

        // Measure echo pulse width
        int64_t start = 0, end = 0;

        while (gpio_get_level(ECHO_PIN) == 0)
        {
            start = esp_timer_get_time();
        }
        while (gpio_get_level(ECHO_PIN) == 1)
        {
            end = esp_timer_get_time();
        }

        // Calculate distance in cm
        int64_t duration = end - start; // in microseconds
        float distance = (duration * 0.034) / 2;

        printf("Distance: %.2f cm\n", distance);

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}