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

# ------------------- 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)
paro_activo = False
estado_anterior = 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 ----------
    actual = emergencia.value()
    if actual == 1 and estado_anterior == 0:
        paro_activo = not paro_activo
        sleep(0.2)
    estado_anterior = actual

    if paro_activo:
        ledr.value(0)
        leda.value(0)
        ledv.value(0)
        for lcd in (lcdr, lcda, lcdv):
            lcd.clear()
            lcd.putstr("SISTEMA STOP\nEMERGENCIA")
        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 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 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 else "DIST: ERR")
    print (estado_v)
    print (dv)

    sleep(0.4)