#include <stdio.h>
#include <stdbool.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <driver/gpio.h>
#include "ultrasonic.h"

// Definições dos pinos (Wokwi)
#define ECHO_GPIO 12
#define TRIGGER_GPIO 13
#define LED_VERDE 15
#define LED_AMARELO 2
#define LED_VERMELHO 4
#define MAX_DISTANCE_CM 100 // 1 metro para modo emergência

// Variáveis globais
static bool modo_emergencia = false;

// Função para controlar os LEDs
static void controlar_leds(bool verde, bool amarelo, bool vermelho) {
    gpio_set_level(LED_VERDE, verde);
    gpio_set_level(LED_AMARELO, amarelo);
    gpio_set_level(LED_VERMELHO, vermelho);
}

// Task do semáforo
static void task_semaforo(void *pvParameters) {
    while(1) {
        if(!modo_emergencia) {
            // Fase verde (10s)
            controlar_leds(1, 0, 0);
            vTaskDelay(pdMS_TO_TICKS(10000));
            
            if(!modo_emergencia) {
                // Fase amarela (1s)
                controlar_leds(0, 1, 0);
                vTaskDelay(pdMS_TO_TICKS(1000));
                
                if(!modo_emergencia) {
                    // Fase vermelha (5s)
                    controlar_leds(0, 0, 1);
                    vTaskDelay(pdMS_TO_TICKS(5000));
                }
            }
        } else {
            vTaskDelay(pdMS_TO_TICKS(100));
        }
    }
}

// Task do sensor ultrassônico (modificada)
static void task_sensor(void *pvParameters) {
    ultrasonic_sensor_t sensor = {
        .trigger_pin = TRIGGER_GPIO,
        .echo_pin = ECHO_GPIO
    };
    ultrasonic_init(&sensor);
    float distance;

    while(1) {
        esp_err_t res = ultrasonic_measure(&sensor, MAX_DISTANCE_CM * 2, &distance);
        
        if(res == ESP_OK) {
            printf("Distância: %.2f cm\n", distance * 100);
            
            if(distance < 1.0 && !modo_emergencia) {
                modo_emergencia = true;
                printf("MODO EMERGÊNCIA ATIVADO!\n");
                controlar_leds(0, 0, 1); // Vermelho imediatamente
            } 
            else if(distance >= 1.0 && modo_emergencia) {
                modo_emergencia = false;
                printf("Retornando ao modo normal\n");
            }
        }
        vTaskDelay(pdMS_TO_TICKS(200));
    }
}

void loop() {
    // Configuração dos GPIOs para os LEDs
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << LED_VERDE) | (1ULL << LED_AMARELO) | (1ULL << LED_VERMELHO),
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&io_conf);

    // Inicia com todos os LEDs apagados
    controlar_leds(0, 0, 0);

    // Cria as tasks
    xTaskCreate(task_semaforo, "semaforo", 4096, NULL, 5, NULL);
    xTaskCreate(task_sensor, "sensor", 4096, NULL, 6, NULL);

    printf("Sistema semafórico inicializado\n");
}