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

# ------------------- LCD I2C -------------------
# I2C0 en Pico: SDA=GP0, SCL=GP1 (como lo tenías)
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=100000)

devices = i2c.scan()
if not devices:
    print("ERROR: No hay dispositivos I2C (LCD no detectado). Revisa SDA/SCL y VCC/GND.")
    while True:
        sleep(1)


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


I2C_ADDR_R   # normalmente 0x27 en LCD1602 I2C [2](https://wokwi.com/projects/365776258692714497)
lcdr = I2cLcd(i2c, I2C_ADDR_R, 2, 16)
lcdr.clear()
lcdr.putstr("LCD OK!")
sleep(2)
lcdr.clear()

I2C_ADDR_A   # normalmente 0x26 en LCD1602 I2C [2](https://wokwi.com/projects/365776258692714497)
lcda = I2cLcd(i2c, I2C_ADDR_A, 2, 16)
lcda.clear()
lcda.putstr("LCD OK!")
sleep(2)
lcda.clear()

I2C_ADDR_V   # normalmente 0x25 en LCD1602 I2C [2](https://wokwi.com/projects/365776258692714497)
lcdv = I2cLcd(i2c, I2C_ADDR_V, 2, 16)
lcdv.clear()
lcdv.putstr("LCD OK!")
sleep(2)
lcdv.clear()


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

#Estados

# Parada de emergencia: botón a GND + Pull-Up interno
emergencia = Pin(12, Pin.IN, Pin.PULL_DOWN)


# Sensor ultrasónico Estado Medio (led/amarillo)
trig_a = Pin(11, Pin.OUT)
echo_a = Pin(10, Pin.IN)

# Sensor ultrasónico Estado Medio (led/rojo)
trig_r = Pin(9, Pin.OUT)
echo_r = Pin(8, Pin.IN)

# Sensor ultrasónico Estado Medio (led/verde)
trig_v = Pin(7, Pin.OUT)
echo_v = Pin(6, Pin.IN)


paro_activo = False          # Estado del sistema
estado_anterior = 0




def obtener_estado_desde_leds():
    if leda.value() == 1:
        return "NIVEL: MEDIO"
    elif ledv.value() == 1:        
        return "NIVEL: ALTO"
    elif ledr.value() == 1:
        return "NIVEL: BAJO"
    else:
        return "NIVEL: PROMEDIO"  # ninguno encendido


def medir_distancia_a():
    """Devuelve distancia en cm, o None si falla/timeout."""
    trig_a.low()
    sleep_us(2)
    trig_a.high()
    sleep_us(10)
    trig_a.low()

    try:
        duracion = time_pulse_us(echo_a, 1, 30000)  # 30ms timeout
        if duracion < 0:
            return None
        return duracion / 58.0  # cm aprox en HC-SR04
    except:
        return None



def medir_distancia_r():
    """Devuelve distancia en cm, o None si falla/timeout."""
    trig_r.low()
    sleep_us(2)
    trig_r.high()
    sleep_us(10)
    trig_r.low()

    try:
        duracion = time_pulse_us(echo_r, 1, 30000)  # 30ms timeout
        if duracion < 0:
            return None
        return duracion / 58.0  # cm aprox en HC-SR04
    except:
        return None




def medir_distancia_v():
    """Devuelve distancia en cm, o None si falla/timeout."""
    trig_v.low()
    sleep_us(2)
    trig_v.high()
    sleep_us(10)
    trig_v.low()

    try:
        duracion = time_pulse_us(echo_v, 1, 30000)  # 30ms timeout
        if duracion < 0:
            return None
        return duracion / 58.0  # cm aprox en HC-SR04
    except:
        return None



while True:
    
    

    # -------- EMERGENCIA CON TOGGLE --------
    estado_actual = emergencia.value()


    # Detectar pulsado (flanco ascendente)
    if estado_actual == 1 and estado_anterior == 0:
        paro_activo = not paro_activo    # Cambia de estado
        print("SISTEMA:", "DESACTIVADO" if paro_activo else "ACTIVADO")
        sleep(0.2)  # Antirebote

    estado_anterior = estado_actual

    # -------- PARO ACTIVO --------
    if paro_activo:
        ledr.value(0)
        leda.value(0)
        ledv.value(0)
        sleep(0.2)        
        lcdr.clear()
        lcda.clear()
        lcdv.clear()
        lcdr.putstr("SISTEMA STOP")
        lcdr.move_to(0, 0)
        lcdr.putstr("EMERGENCIA")
        lcda.putstr("SISTEMA STOP")
        lcda.move_to(0, 0)
        lcda.putstr("EMERGENCIA")
        lcdv.putstr("SISTEMA STOP")
        lcdv.move_to(0, 0)
        lcdv.putstr("EMERGENCIA")          
        sleep(0.2)
        continue

             

    distancia_a = medir_distancia_a()
    distancia_r = medir_distancia_r()
    distancia_v = medir_distancia_v()
    
    estado = obtener_estado_desde_leds()


    #Pantalla LED rojo

    distancia_r = medir_distancia_r()
    estado = obtener_estado_desde_leds()
    print (estado)
    # ---- Mostrar en LCD (si falla sensor, muestra ERR) ----
    lcdr.clear()
    lcdr.move_to(0, 1)
    lcdr.putstr("NIVEL| {:5.1f}cm".format(distancia_r))
    lcdr.move_to(1, 0)
    lcdr.putstr(obtener_estado_desde_leds())


    lcda.clear()
    lcda.move_to(0, 1)
    lcda.putstr("NIVEL| {:5.1f}cm".format(distancia_a))
    lcda.move_to(1, 0)
    lcda.putstr(obtener_estado_desde_leds())

    lcdv.clear()
    lcdv.move_to(0, 1)
    lcdv.putstr("NIVEL| {:5.1f}cm".format(distancia_v))
    lcdv.move_to(1, 0)
    lcdv.putstr(obtener_estado_desde_leds())


    if distancia_r is None:
        lcdr.putstr("NIVEL| ERR")
        print("NIVEL | ERR")

    else:
        
        print("NIVEl", distancia_r, "cm")
        
        sleep(0.4)


    lcdr.clear()
    lcdr.move_to(0, 0)
    lcdr.putstr("ESTADO: " + estado)
    lcdr.move_to(0, 1)
    if distancia_r is None:
        lcdr.putstr("DIST: ERROR")
    else:
        lcdr.putstr("DIST: {:5.1f}cm".format(distancia_r))




# ---- Lógica de loop

    if  distancia_r <= 100:
        ledr.value (1)


    
    elif distancia_a <= 200:
        
        leda.value (1)
        

    elif distancia_v  <= 400:
        
        ledv.value (1)

    else: 
    
       ledr.value (0)
       leda.value (0)
       ledv.value (0)