# 3 LCD + 3 Sensores (Pico / MicroPython) + Control por rangos
# + Encendido/Apagado + Parada de Emergencia (con debounce)
# -------------------------
from machine import Pin, I2C, SoftI2C, time_pulse_us
from time import sleep, sleep_us, ticks_ms, ticks_diff
from pico_i2c_lcd import I2cLcd

# -------------------------
# I2C LCD 1 (Hardware I2C0) -> GP0/GP1
# -------------------------
i2c0 = I2C(0, sda=Pin(0), scl=Pin(1), freq=100000)
addrs0 = i2c0.scan()
if not addrs0:
    raise RuntimeError("No se detecta LCD en I2C0 (SDA=GP0, SCL=GP1).")
I2C_ADDR0 = addrs0[0]
lcd1 = I2cLcd(i2c0, I2C_ADDR0, 2, 16)

# -------------------------
# I2C LCD 2 (Hardware I2C1) -> GP18/GP19
# -------------------------
i2c1 = I2C(1, sda=Pin(18), scl=Pin(19), freq=100000)
addrs1 = i2c1.scan()
if not addrs1:
    raise RuntimeError("No se detecta LCD en I2C1 (SDA=GP18, SCL=GP19).")
I2C_ADDR1 = addrs1[0]
lcd2 = I2cLcd(i2c1, I2C_ADDR1, 2, 16)

# -------------------------
# I2C LCD 3 (SoftI2C) -> GP20/GP21
# -------------------------
i2c2 = SoftI2C(sda=Pin(20), scl=Pin(21), freq=100000)
addrs2 = i2c2.scan()
if not addrs2:
    raise RuntimeError("No se detecta LCD en SoftI2C (SDA=GP20, SCL=GP21).")
I2C_ADDR2 = addrs2[0]
lcd3 = I2cLcd(i2c2, I2C_ADDR2, 2, 16)

# -------------------------
# Sensores ultrasónicos
# -------------------------
TRIG1 = Pin(7, Pin.OUT)
ECHO1 = Pin(6, Pin.IN)

TRIG2 = Pin(11, Pin.OUT)
ECHO2 = Pin(10, Pin.IN)

TRIG3 = Pin(9, Pin.OUT)
ECHO3 = Pin(8, Pin.IN)

# -------------------------
# Rangos de control
# -------------------------
R1_MIN, R1_MAX = 25, 50     # d1 -> LED rojo
R2_MIN, R2_MAX = 51, 75     # d2 -> LED amarillo
R3_MIN, R3_MAX = 76, 100    # d3 -> LED verde

# -------------------------
# LEDs (independientes)
# -------------------------
ledr = Pin(15, Pin.OUT)   # Rojo
leda = Pin(14, Pin.OUT)   # Amarillo
ledv = Pin(13, Pin.OUT)   # Verde

# Apagar LEDs al inicio
ledr.value(0); leda.value(0); ledv.value(0)

# -------------------------
# Botones: Emergencia / Encendido / Apagado
# (Configurados con PULL_DOWN: reposo=0, pulsado=1)
# -------------------------
emergencia = Pin(12, Pin.IN, Pin.PULL_DOWN)   # Botón Rojo (E-Stop)
encendido  = Pin(16, Pin.IN, Pin.PULL_DOWN)   # Botón Verde (ON)
apagado    = Pin(17, Pin.IN, Pin.PULL_DOWN)   # Botón Azul  (OFF)

# Estados
paro_activo = False        # Se activa con emergencia
sistema_activo = False     # Arranca apagado

# Antirrebote (debounce)
DEBOUNCE_MS = 150
enc_anterior = 0
apag_anterior = 0
eme_anterior = 0

last_enc_ms = 0
last_apag_ms = 0
last_eme_ms = 0

def boton_pulsado(pin, estado_anterior, last_ms):
    """
    Detecta flanco de subida (0->1) con debounce.
    Retorna: (evento_pulsado, nuevo_estado_anterior, nuevo_last_ms)
    """
    ahora = ticks_ms()
    estado = pin.value()

    evento = False
    if estado != estado_anterior:
        # Cambio detectado, verificar debounce
        if ticks_diff(ahora, last_ms) > DEBOUNCE_MS:
            last_ms = ahora
            # Evento solo cuando se PRESIONA (0->1)
            if estado_anterior == 0 and estado == 1:
                evento = True
            estado_anterior = estado

    return evento, estado_anterior, last_ms

def medir_distancia(trig: Pin, echo: Pin, timeout_us=30000):
    trig.low()
    sleep_us(2)
    trig.high()
    sleep_us(10)
    trig.low()

    dur = time_pulse_us(echo, 1, timeout_us)
    if dur < 0:
        return None

    dist_cm = (dur * 0.0343) / 2
    return dist_cm

def en_rango(valor, minimo, maximo):
    if valor is None:
        return False
    return minimo <= valor <= maximo

def mostrar(lcd, titulo, dist, ok):
    lcd.clear()
    lcd.putstr(titulo + "\n")
    if dist is None:
        lcd.putstr("Sin eco")
    else:
        estado = "OK" if ok else "FUERA"
        lcd.putstr("{:0.1f}cm {}".format(dist, estado))

def mostrar_estado_global(msg_linea1, msg_linea2=""):
    # Muestra un estado en las 3 pantallas
    for lcd in (lcd1, lcd2, lcd3):
        lcd.clear()
        lcd.putstr(msg_linea1 + "\n")
        lcd.putstr(msg_linea2)

# Títulos
tit1 = "Nivel S1:"
tit2 = "Nivel S2:"
tit3 = "Nivel S3:"

# -------------------------
# Loop principal
# -------------------------
while True:
    # ---- Leer botones con debounce y flanco ----
    evt_eme, eme_anterior, last_eme_ms = boton_pulsado(emergencia, eme_anterior, last_eme_ms)
    evt_on,  enc_anterior, last_enc_ms = boton_pulsado(encendido,  enc_anterior, last_enc_ms)
    evt_off, apag_anterior, last_apag_ms = boton_pulsado(apagado, apag_anterior, last_apag_ms)

    # ---- Parada de emergencia ----
    # Si se presiona emergencia: se detiene el sistema y queda enclavado (paro_activo=True)
    if evt_eme:
        paro_activo = True
        sistema_activo = False

    # ---- Encender / Apagar ----
    # Encender solo si NO hay paro activo y el botón de emergencia NO está presionado
    if evt_on and (not paro_activo) and (emergencia.value() == 0):
        sistema_activo = True

    # Apagar siempre que se presione OFF (aunque esté en paro, mantiene apagado)
    if evt_off:
        sistema_activo = False

    # ---- Rearme del paro (simple y seguro) ----
    # Para salir de paro:
    # 1) Sueltas el botón de emergencia (emergencia.value()==0)
    # 2) Presionas ENCENDIDO (evt_on) -> arriba ya lo permite solo si paro_activo=False
    # Entonces: aquí liberamos el paro cuando la emergencia está suelta y presionas ON.
    # (Si prefieres que se rearme automáticamente al soltar emergencia, te lo cambio.)
    if evt_on and paro_activo and (emergencia.value() == 0):
        paro_activo = False
        sistema_activo = True

    # ---- Si paro activo: mostrar y apagar todo ----
    if paro_activo:
        ledr.value(0); leda.value(0); ledv.value(0)
        mostrar_estado_global("!!! EMERG !!!", "Sistema Parado")
        sleep(0.2)
        continue

    # ---- Si sistema apagado: mostrar OFF y apagar LEDs ----
    if not sistema_activo:
        ledr.value(0); leda.value(0); ledv.value(0)
        mostrar_estado_global("Sistema OFF", "Presiona ON")
        sleep(0.2)
        continue

    # -------------------------
    # Sistema activo: medir y controlar
    # -------------------------
    d1 = medir_distancia(TRIG1, ECHO1); sleep(0.07)
    d2 = medir_distancia(TRIG2, ECHO2); sleep(0.07)
    d3 = medir_distancia(TRIG3, ECHO3); sleep(0.07)

    ok1 = en_rango(d1, R1_MIN, R1_MAX)
    ok2 = en_rango(d2, R2_MIN, R2_MAX)
    ok3 = en_rango(d3, R3_MIN, R3_MAX)

    # LEDs INDEPENDIENTES
    ledr.value(1 if ok1 else 0)
    leda.value(1 if ok2 else 0)
    ledv.value(1 if ok3 else 0)

    # LCDs
    mostrar(lcd1, tit1, d1, ok1)
    mostrar(lcd2, tit2, d2, ok2)
    mostrar(lcd3, tit3, d3, ok3)

    # Consola
    print("S1:", d1 if d1 is not None else "Sin eco", "->", "OK" if ok1 else "FUERA",
          "| S2:", d2 if d2 is not None else "Sin eco", "->", "OK" if ok2 else "FUERA",
          "| S3:", d3 if d3 is not None else "Sin eco", "->", "OK" if ok3 else "FUERA",
          "| Sistema:", "ON" if sistema_activo else "OFF",
          "| Paro:", "SI" if paro_activo else "NO")

    sleep(0.2)