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 = devices[0]   # normalmente 0x27 en LCD1602 I2C [2](https://wokwi.com/projects/365776258692714497)
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)
lcd.clear()
lcd.putstr("LCD OK!")
sleep(2)
lcd.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)        
        lcd.clear()
        lcd.putstr("SISTEMA STOP")
        lcd.move_to(0, 0)
        lcd.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()



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

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

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



# ---- Lógica de loop

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

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

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

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

        
        sleep(0.4)