#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/timer.h"

#define TRIG_PIN 10
#define ECHO_PIN 11
#define LED_VERDE 18
#define LED_AMARELO 17
#define LED_VERMELHO 16
#define BOTAO_TESTE 20
#define DISTANCIA_LIMITE 50 // cm para detectar veículos

void configurar_gpio() {
    gpio_init(LED_VERDE);
    gpio_init(LED_AMARELO);
    gpio_init(LED_VERMELHO);
    gpio_init(TRIG_PIN);
    gpio_init(ECHO_PIN);
    gpio_init(BOTAO_TESTE);
    
    gpio_set_dir(LED_VERDE, GPIO_OUT);
    gpio_set_dir(LED_AMARELO, GPIO_OUT);
    gpio_set_dir(LED_VERMELHO, GPIO_OUT);
    gpio_set_dir(TRIG_PIN, GPIO_OUT);
    gpio_set_dir(ECHO_PIN, GPIO_IN);
    gpio_set_dir(BOTAO_TESTE, GPIO_IN);
    
    printf("GPIO configurado\n");
}

void acender_led(int led) {
    gpio_put(LED_VERDE, 0);
    gpio_put(LED_AMARELO, 0);
    gpio_put(LED_VERMELHO, 0);
    gpio_put(led, 1);
    
    if (led == LED_VERDE) printf("LED Verde aceso\n");
    else if (led == LED_AMARELO) printf("LED Amarelo aceso\n");
    else if (led == LED_VERMELHO) printf("LED Vermelho aceso\n");
}

int medir_distancia() {
    gpio_put(TRIG_PIN, 1);
    sleep_us(10);
    gpio_put(TRIG_PIN, 0);
    
    while (gpio_get(ECHO_PIN) == 0);
    absolute_time_t start_time = get_absolute_time();
    while (gpio_get(ECHO_PIN) == 1);
    absolute_time_t end_time = get_absolute_time();
    
    int64_t tempo = absolute_time_diff_us(start_time, end_time);
    int distancia = tempo / 58; // Conversão para cm
    
    printf("Distância medida: %d cm\n", distancia);
    return distancia;
}

void controlar_semaforo() {
    int distancia = medir_distancia();
    int veiculo_prioritario = gpio_get(BOTAO_TESTE);
    
    if (veiculo_prioritario) {
        printf("Veículo prioritário detectado\n");
        acender_led(LED_VERDE);
        sleep_ms(5000);
    } else if (distancia < DISTANCIA_LIMITE) {
        printf("Veículo comum detectado\n");
        acender_led(LED_VERDE);
        sleep_ms(5000);
        acender_led(LED_AMARELO);
        sleep_ms(2000);
    }
    printf("Mudando para vermelho\n");
    acender_led(LED_VERMELHO);
    sleep_ms(5000);
}

void testar_sistema() {
    printf("Iniciando testes de validação\n");
    
    printf("Teste 1: Sem veículos - Esperado: Semáforo vermelho\n");
    controlar_semaforo();
    
    printf("Teste 2: Veículo comum detectado - Esperado: Verde -> Amarelo -> Vermelho\n");
    gpio_put(TRIG_PIN, 1);
    sleep_ms(100);
    gpio_put(TRIG_PIN, 0);
    controlar_semaforo();
    
    printf("Teste 3: Veículo prioritário detectado - Esperado: Verde por 5s\n");
    gpio_put(BOTAO_TESTE, 1);
    controlar_semaforo();
    gpio_put(BOTAO_TESTE, 0);
    
    printf("Testes concluídos\n");
}

int main() {
    stdio_init_all();
    configurar_gpio();
    
    printf("Iniciando o sistema de semáforo\n");
    testar_sistema();
    
    while (1) {
        controlar_semaforo();
    }
}