from machine import Pin, I2C
import ssd1306
import utime

# ============================
# CONFIGURACIÓN DE HARDWARE
# ============================

# OLED I2C
i2c = I2C(0, scl=Pin(5), sda=Pin(4))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

# Sensores ultrasónicos
TRIG_A = Pin(2, Pin.OUT)
ECHO_A = Pin(3, Pin.IN)
TRIG_B = Pin(6, Pin.OUT)
ECHO_B = Pin(7, Pin.IN)

# Semáforo A
red_A = Pin(10, Pin.OUT)
yellow_A = Pin(11, Pin.OUT)
green_A = Pin(12, Pin.OUT)

# Semáforo B
red_B = Pin(13, Pin.OUT)
yellow_B = Pin(14, Pin.OUT)
green_B = Pin(15, Pin.OUT)

# ============================
# FUNCIONES
# ============================

def medir_distancia(trig, echo, timeout=30000):
    """Mide la distancia con el sensor ultrasónico en cm, con timeout de seguridad."""
    trig.low()
    utime.sleep_us(2)
    trig.high()
    utime.sleep_us(10)
    trig.low()

    inicio = utime.ticks_us()
    while echo.value() == 0:
        if utime.ticks_diff(utime.ticks_us(), inicio) > timeout:
            return 999  # no se detectó nada
    start = utime.ticks_us()

    while echo.value() == 1:
        if utime.ticks_diff(utime.ticks_us(), inicio) > timeout:
            return 999
    end = utime.ticks_us()

    duracion = end - start
    distancia = (duracion * 0.0343) / 2
    return distancia

def clasificar_trafico(distancia):
    """Clasifica el tráfico según la distancia detectada."""
    if distancia > 150:
        return "Ligero", 5
    elif distancia > 60:
        return "Pesado", 8
    else:
        return "Saturado", 12

def mostrar_trafico(estadoA, distA, estadoB, distB):
    """Muestra el estado del tráfico y distancias en la OLED."""
    oled.fill(0)
    oled.text("Semaforo A:", 0, 0)
    oled.text("Dist:{} cm".format(int(distA)), 0, 10)
    oled.text("Traf: {}".format(estadoA), 0, 20)

    oled.text("Semaforo B:", 0, 35)
    oled.text("Dist:{} cm".format(int(distB)), 0, 45)
    oled.text("Traf: {}".format(estadoB), 0, 55)
    oled.show()

def semaforo_A(t_verde):
    """Secuencia del semáforo A."""
    # Verde A
    red_A.off(); yellow_A.off(); green_A.on()
    red_B.on(); yellow_B.off(); green_B.off()
    utime.sleep(t_verde)

    # Amarillo A
    green_A.off(); yellow_A.on()
    utime.sleep(2)

    # Rojo A
    yellow_A.off(); red_A.on()

def semaforo_B(t_verde):
    """Secuencia del semáforo B."""
    # Verde B
    red_B.off(); yellow_B.off(); green_B.on()
    red_A.on(); yellow_A.off(); green_A.off()
    utime.sleep(t_verde)

    # Amarillo B
    green_B.off(); yellow_B.on()
    utime.sleep(2)

    # Rojo B
    yellow_B.off(); red_B.on()

# ============================
# PROGRAMA PRINCIPAL
# ============================

try:
    while True:
        # Leer sensores
        distA = medir_distancia(TRIG_A, ECHO_A)
        distB = medir_distancia(TRIG_B, ECHO_B)

        # Clasificar tráfico
        estadoA, tiempoA = clasificar_trafico(distA)
        estadoB, tiempoB = clasificar_trafico(distB)

        # Mostrar en OLED
        mostrar_trafico(estadoA, distA, estadoB, distB)

        # Decidir quién pasa primero según tráfico
        if estadoA == "Saturado" or (estadoA == "Pesado" and estadoB == "Ligero"):
            semaforo_A(tiempoA)
            semaforo_B(tiempoB)
        else:
            semaforo_B(tiempoB)
            semaforo_A(tiempoA)

except KeyboardInterrupt:
    oled.fill(0); oled.show()
    red_A.off(); yellow_A.off(); green_A.off()
    red_B.off(); yellow_B.off(); green_B.off()
    print("Programa detenido")