from machine import Pin
import time

# Configuración del sensor ultrasónico
trigger = Pin(5, Pin.OUT)
echo = Pin(18, Pin.IN)

# Configuración de LEDs del semáforo
led_rojo = Pin(23, Pin.OUT)
led_amarillo = Pin(22, Pin.OUT)
led_verde = Pin(21, Pin.OUT)

def medir_distancia():
    try:
        # Enviar pulso trigger
        trigger.value(0)
        time.sleep_us(2)
        trigger.value(1)
        time.sleep_us(10)
        trigger.value(0)
        
        # Esperar que echo se active
        timeout = 0
        while echo.value() == 0 and timeout < 10000:
            timeout += 1
        
        if timeout >= 10000:
            return 999  # Error de timeout
            
        inicio = time.ticks_us()
        
        # Esperar que echo se desactive
        timeout = 0
        while echo.value() == 1 and timeout < 10000:
            timeout += 1
            
        if timeout >= 10000:
            return 999  # Error de timeout
            
        fin = time.ticks_us()
        
        # Calcular distancia en cm
        duracion = time.ticks_diff(fin, inicio)
        distancia = (duracion * 0.034) / 2
        return distancia
    except:
        return 999  # Error

def semaforo_normal():
    # Ciclo normal del semáforo
    print("Semáforo en modo normal")
    
    # Rojo
    led_verde.value(0)
    led_amarillo.value(0)
    led_rojo.value(1)
    time.sleep(3)
    
    # Amarillo
    led_verde.value(0)
    led_amarillo.value(1)
    time.sleep(1)
    
    # Verde
    led_verde.value(1)
    led_amarillo.value(0)
    led_rojo.value(0)
    time.sleep(2)

    # Amarillo
    led_verde.value(0)
    led_amarillo.value(1)
    time.sleep(1)

def detener_semaforo():
    # Mantener luz roja cuando hay vehículo
    led_rojo.value(1)
    led_verde.value(0)
    led_amarillo.value(0)

# Programa principal
print("=== Semáforo Inteligente Iniciado ===")

while True:
    distancia = medir_distancia()
    
    if distancia < 999:  # Lectura válida
        print("Distancia: {:.1f} cm".format(distancia))
        
        if distancia < 200:  # Vehículo detectado
            print("¡Vehículo detectado! - Semáforo en ROJO")
            detener_semaforo()
            time.sleep(1)
        else:
            semaforo_normal()
    else:
        print("Error en sensor - Modo seguro")
        detener_semaforo()
        time.sleep(1)
    
    time.sleep(0.1)