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

// --- Configuration ---
// **REMEMBER THE VOLTAGE DIVIDER ON THE ECHO PIN!**
#define TRIGGER_GPIO GPIO_NUM_5  // ESP32-S3 GPIO pin connected to HC-SR04 TRIG
#define ECHO_GPIO    GPIO_NUM_18 // ESP32-S3 GPIO pin connected to HC-SR04 ECHO (via voltage divider)
#define MAX_DISTANCE_CM 400.0f   // Maximum measurement distance
#define LOG_TAG "ULTRASONIC_APP"

// --- Ultrasonic Driver (Single File Version) ---

// Sentinel value for timeout/error
#define ULTRASONIC_NO_RESPONSE 99999.0f 
// Speed of sound in cm/us (at 20C). The formula is Duration / 58.2
#define SPEED_OF_SOUND_CM_PER_US 0.0343f
#define DURATION_TO_CM_DIVISOR (2.0f / SPEED_OF_SOUND_CM_PER_US) // ~ 58.2

/**
 * @brief Measures the distance using the HC-SR04 sensor.
 * * @param trigger_pin GPIO pin connected to TRIG.
 * @param echo_pin GPIO pin connected to ECHO.
 * @param max_distance_cm Maximum distance to measure (for calculating timeout).
 * @param distance Pointer to a float where the measured distance in cm will be stored.
 * @return esp_err_t ESP_OK if successful, ESP_ERR_TIMEOUT if out of range/no response.
 */
esp_err_t measure_distance_cm(gpio_num_t trigger_pin, gpio_num_t echo_pin, float max_distance_cm, float *distance)
{
    if (!distance)
        return ESP_ERR_INVALID_ARG;

    *distance = ULTRASONIC_NO_RESPONSE;

    // Calculate maximum wait time (timeout) based on max_distance_cm
    // Max Time (us) = max_distance_cm * DURATION_TO_CM_DIVISOR
    uint64_t max_duration_us = (uint64_t)(max_distance_cm * DURATION_TO_CM_DIVISOR);
    if (max_duration_us < 100) max_duration_us = 100;

    // 1. Send the trigger pulse (10 us HIGH)
    gpio_set_level(trigger_pin, 0);
    ets_delay_us(2);
    gpio_set_level(trigger_pin, 1);
    ets_delay_us(10);
    gpio_set_level(trigger_pin, 0);

    uint64_t start_time = 0;
    uint64_t end_time = 0;
    uint64_t current_time;

    // 2. Wait for ECHO pin RISING edge (Start of pulse)
    uint64_t timeout_start = esp_timer_get_time();
    while (gpio_get_level(echo_pin) == 0) {
        if ((esp_timer_get_time() - timeout_start) > max_duration_us + 1000) {
            return ESP_ERR_TIMEOUT; // Timeout waiting for signal start
        }
        ets_delay_us(1); 
    }
    start_time = esp_timer_get_time();

    // 3. Wait for ECHO pin FALLING edge (End of pulse)
    while (gpio_get_level(echo_pin) == 1) {
        current_time = esp_timer_get_time();
        if ((current_time - start_time) > max_duration_us) {
            return ESP_ERR_TIMEOUT; // Timeout waiting for signal end
        }
        ets_delay_us(1); 
    }
    end_time = esp_timer_get_time();

    // 4. Calculate Duration and Distance
    uint64_t duration_us = end_time - start_time;
    
    // Distance (cm) = Duration in us / 58.2
    *distance = (float)duration_us / DURATION_TO_CM_DIVISOR;

    return ESP_OK;
}

/**
 * @brief Initializes the GPIO pins.
 */
void ultrasonic_init_gpio(gpio_num_t trigger_pin, gpio_num_t echo_pin)
{
    // Configure TRIG pin as output
    gpio_reset_pin(trigger_pin);
    gpio_set_direction(trigger_pin, GPIO_MODE_OUTPUT);
    gpio_set_level(trigger_pin, 0); 

    // Configure ECHO pin as input
    gpio_reset_pin(echo_pin);
    gpio_set_direction(echo_pin, GPIO_MODE_INPUT);

    ESP_LOGI(LOG_TAG, "GPIO initialized. TRIG: %d, ECHO: %d (via divider!)", trigger_pin, echo_pin);
}

// --- Application Task ---

void ultrasonic_task(void *pvParameters)
{
    // 1. Initialize the GPIOs
    ultrasonic_init_gpio(TRIGGER_GPIO, ECHO_GPIO);
    
    ESP_LOGI(LOG_TAG, "Starting distance measurement loop...");

    while (1) {
        float distance_cm;
        
        // 2. Measure the distance
        esp_err_t res = measure_distance_cm(TRIGGER_GPIO, ECHO_GPIO, MAX_DISTANCE_CM, &distance_cm);

        if (res == ESP_OK) {
            printf("Distance: **%.2f cm**\n", distance_cm);
        } else if (res == ESP_ERR_TIMEOUT) {
            printf("Distance: **Out of Range (> %.0f cm) or Timeout**\n", MAX_DISTANCE_CM);
        } else {
            ESP_LOGE(LOG_TAG, "Error during measurement: %s", esp_err_to_name(res));
        }

        // Wait before the next measurement
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void app_main(void)
{
    // Create the task to handle ultrasonic readings
    xTaskCreate(ultrasonic_task, "ultrasonic_task", 4096, NULL, 5, NULL);
}