from machine import Pin, I2C, time_pulse_us
from time import sleep, sleep_us, ticks_ms, ticks_diff
from pico_i2c_lcd import I2cLcd

import ujson
cfg = {"lcd_r": "0x27", "lcd_a": "0x26", "lcd_v": "0x25"}
print(ujson.dumps(cfg))

# ------------------- LCD I2C -------------------
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=100000)

if not i2c.scan():
    print("ERROR I2C")
    while True:
        sleep(1)

I2C_ADDR_R = 0x27
I2C_ADDR_A = 0x26
I2C_ADDR_V = 0x25

lcdr = I2cLcd(i2c, I2C_ADDR_R, 2, 16)
lcda = I2cLcd(i2c, I2C_ADDR_A, 2, 16)
lcdv = I2cLcd(i2c, I2C_ADDR_V, 2, 16)

# ------------------- LEDs -------------------
ledr = Pin(15, Pin.OUT)   # BAJO
leda = Pin(14, Pin.OUT)   # MEDIO
ledv = Pin(13, Pin.OUT)   # ALTO

# ------------------- Emergencia -------------------
emergencia = Pin(12, Pin.IN, Pin.PULL_DOWN)  # Botón Rojo

# ------------------- Encendido -------------------
encendido = Pin(16, Pin.IN, Pin.PULL_DOWN)     # Botón Verde (PULL_UP: suelto=1, pulsado=0)

# ------------------- Apagado -------------------
apagado = Pin(17, Pin.IN, Pin.PULL_DOWN)       # Botón Azul (PULL_UP: suelto=1, pulsado=0)

paro_activo = False
estado_anterior = 0

# ------------------- NUEVO: Estado del sistema -------------------
sistema_activo = False  # Arranca apagado (puedes poner True si quieres que inicie encendido)

# ------------------- NUEVO: Antirrebote y flancos para botones -------------------
enc_anterior = 1        # por ser PULL_UP, en reposo es 1
apag_anterior = 1

DEBOUNCE_MS = 150
last_enc_ms = 0
last_apag_ms = 0

# ------------------- Sensores -------------------
trig_r = Pin(9, Pin.OUT)
echo_r = Pin(8, Pin.IN)

trig_a = Pin(11, Pin.OUT)
echo_a = Pin(10, Pin.IN)

trig_v = Pin(7, Pin.OUT)
echo_v = Pin(6, Pin.IN)

# ------------------- Función medición -------------------
def medir(trig, echo):
    trig.low()
    sleep_us(2)
    trig.high()
    sleep_us(10)
    trig.low()
    try:
        t = time_pulse_us(echo, 1, 30000)
        if t < 0:
            return None
        return t / 58
    except:
        return None

# =================== LOOP ===================
while True:


    # ---------- EMERGENCIA (toggle por flanco) ----------
    actual = emergencia.value()
    if actual == 1 and estado_anterior == 0:
        paro_activo = not paro_activo
        sleep(0.2)  # antirrebote simple
    estado_anterior = actual

    # ---------- BOTONES ENCENDER / APAGAR (flanco 1->0 + debounce) ----------
    now = ticks_ms()

    enc_actual = encendido.value()
    apag_actual = apagado.value()

    # ENCENDER: detectar flanco de bajada (1 -> 0)
    if enc_anterior == 1 and enc_actual == 0:
        if ticks_diff(now, last_enc_ms) > DEBOUNCE_MS:
            # Si hay emergencia activa, NO permitir encender (seguridad)
            if not paro_activo:
                sistema_activo = True
                print("SISTEMA ENCENDIDO")
            last_enc_ms = now

    # APAGAR: detectar flanco de bajada (1 -> 0)
    if apag_anterior == 1 and apag_actual == 0:
        if ticks_diff(now, last_apag_ms) > DEBOUNCE_MS:
            sistema_activo = False
            print("SISTEMA APAGADO")
            last_apag_ms = now

    enc_anterior = enc_actual
    apag_anterior = apag_actual

    # ---------- PRIORIDAD EMERGENCIA ----------
    if paro_activo:
        sistema_activo = False  # opcional: fuerza apagado cuando hay emergencia
        ledr.value(0)
        leda.value(0)
        ledv.value(0)
        for lcd in (lcdr, lcda, lcdv):
            lcd.clear()
            lcd.putstr("SISTEMA STOP\nEMERGENCIA")
        print("SISTEMA STOP\nEMERGENCIA")
        sleep(0.2)
        continue

    # ---------- SISTEMA APAGADO ----------
    if not sistema_activo:
        ledr.value(0)
        leda.value(0)
        ledv.value(0)
        for lcd in (lcdr, lcda, lcdv):
            lcd.clear()
            lcd.putstr("SISTEMA\nAPAGADO")
        print("SISTEMA APAGADO")
        sleep(0.10)
        continue

    # ---------- MEDICIONES ----------
    dr = medir(trig_r, echo_r)
    da = medir(trig_a, echo_a)
    dv = medir(trig_v, echo_v)

    # ---------- LED ROJO (BAJO) ----------
    ledr.value(0)
    lcdr.clear()
    if dr is not None and 30 <= dr <= 50:
        ledr.value(1)
        estado_r = "NIVEL: BAJO"
    else:
        estado_r = "NIVEL: TK-1"
    lcdr.putstr(estado_r)
    lcdr.move_to(0, 1)
    lcdr.putstr("DIST:{:5.1f}".format(dr) if dr is not None else "DIST: ERR")
    print(estado_r)
    print(dr)

    # ---------- LED AMARILLO (MEDIO) ----------
    leda.value(0)
    lcda.clear()
    if da is not None and 70 <= da <= 100:
        leda.value(1)
        estado_a = "NIVEL: MEDIO"
    else:
        estado_a = "NIVEL: TK-2"
    lcda.putstr(estado_a)
    lcda.move_to(0, 1)
    lcda.putstr("DIST:{:5.1f}".format(da) if da is not None else "DIST: ERR")
    print(estado_a)
    print(da)

    # ---------- LED VERDE (ALTO) ----------
    ledv.value(0)
    lcdv.clear()
    if dv is not None and 150 <= dv <= 200:
        ledv.value(1)
        estado_v = "NIVEL: ALTO"
    else:
        estado_v = "NIVEL: TK-3"
    lcdv.putstr(estado_v)
    lcdv.move_to(0, 1)
    lcdv.putstr("DIST:{:5.1f}".format(dv) if dv is not None else "DIST: ERR")
    print(estado_v)
    print(dv)

    sleep(0.4)