#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include "rom/ets_sys.h" // For ets_delay_us

#define TRIG_PIN 5
#define ECHO_PIN 18

void app_main() {
    // 1. Setup Pin configurations
    gpio_set_direction(TRIG_PIN, GPIO_MODE_OUTPUT);
    gpio_set_direction(ECHO_PIN, GPIO_MODE_INPUT);

    while (1) {
        // 2. Clear Trigger
        gpio_set_level(TRIG_PIN, 0);
        ets_delay_us(2);

        // 3. Send 10us Pulse
        gpio_set_level(TRIG_PIN, 1);
        ets_delay_us(10);
        gpio_set_level(TRIG_PIN, 0);

        // 4. Measure Echo (simplified manual implementation of pulseIn)
        // Wait for Echo to go HIGH
        int timeout = 0;
        while (gpio_get_level(ECHO_PIN) == 0) {
            if (timeout++ > 10000) break; // prevent infinite loop
            ets_delay_us(1);
        }

        int64_t start_time = esp_timer_get_time();
        
        // Wait for Echo to go LOW
        timeout = 0;
        while (gpio_get_level(ECHO_PIN) == 1) {
             if (timeout++ > 100000) break; 
             ets_delay_us(1);
        }
        
        int64_t end_time = esp_timer_get_time();

        // 5. Calculate Distance
        int64_t duration = end_time - start_time;
        float distance = (duration * 0.0343) / 2;

        // 6. Print to Console (Standard C uses printf, not Serial.print)
        printf("Distance: %.2f cm\n", distance);

        // Delay 100ms (100 / portTICK_PERIOD_MS)
        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
}