#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_system.h"
#include "esp_log.h"

#define TRIGGER_GPIO 23 // GPIO pin connected to the ultrasonic sensor's trigger
#define ECHO_GPIO 22    // GPIO pin connected to the ultrasonic sensor's echo
#define LED_NORMAL_GPIO 4 // GPIO pin connected to the normal LED
#define LED_FLOOD_GPIO 5  // GPIO pin connected to the flood detection LED

#define MAX_DISTANCE_CM 200 // Maximum distance in centimeters to measure

static const char *TAG = "flood_detection";

void ultrasonic_sensor_init() {
    gpio_pad_select_gpio(TRIGGER_GPIO);
    gpio_set_direction(TRIGGER_GPIO, GPIO_MODE_OUTPUT);

    gpio_pad_select_gpio(ECHO_GPIO);
    gpio_set_direction(ECHO_GPIO, GPIO_MODE_INPUT);
}

float ultrasonic_sensor_get_distance_cm() {
    gpio_set_level(TRIGGER_GPIO, 0); // Ensure trigger is low
    vTaskDelay(2 / portTICK_PERIOD_MS); // Wait for 2 milliseconds

    gpio_set_level(TRIGGER_GPIO, 1); // Send a 10us pulse to trigger
    vTaskDelay(10 / portTICK_PERIOD_MS);
    gpio_set_level(TRIGGER_GPIO, 0);

    // Wait for echo signal
    while (gpio_get_level(ECHO_GPIO) == 0) {}
    int64_t start = esp_timer_get_time();

    while (gpio_get_level(ECHO_GPIO) == 1) {}
    int64_t end = esp_timer_get_time();

    // Calculate distance in centimeters
    float distance = ((end - start) * 0.0343) / 2;

    return distance;
}

void flood_detection_task(void *pvParameter) {
    float distance;

    while (1) {
        distance = ultrasonic_sensor_get_distance_cm();
        ESP_LOGI(TAG, "Distance: %.2f cm", distance);

        if (distance < MAX_DISTANCE_CM) {
            // Flood detected
            gpio_set_level(LED_NORMAL_GPIO, 0); // Turn off normal LED
            gpio_set_level(LED_FLOOD_GPIO, 1);  // Turn on flood detection LED
        } else {
            // No flood
            gpio_set_level(LED_NORMAL_GPIO, 1); // Turn on normal LED
            gpio_set_level(LED_FLOOD_GPIO, 0);  // Turn off flood detection LED
        }

        vTaskDelay(1000 / portTICK_PERIOD_MS); // Delay 1 second before next measurement
    }
}

void app_main() {
    // Initialize GPIOs for LEDs
    gpio_pad_select_gpio(LED_NORMAL_GPIO);
    gpio_set_direction(LED_NORMAL_GPIO, GPIO_MODE_OUTPUT);
    gpio_pad_select_gpio(LED_FLOOD_GPIO);
    gpio_set_direction(LED_FLOOD_GPIO, GPIO_MODE_OUTPUT);

    // Initialize ultrasonic sensor
    ultrasonic_sensor_init();

    // Create task to monitor flood detection
    xTaskCreate(&flood_detection_task, "flood_detection_task", 4096, NULL, 5, NULL);
}