from machine import Pin, I2C, PWM, ADC
from time import sleep
from i2c_lcd import I2cLcd

pot = ADC(Pin(26))         # ADC0
joy = ADC(Pin(27))         # ADC1
servo = PWM(Pin(14), freq=50)  # PWM en GPIO14
pulsador = Pin(15, Pin.IN, Pin.PULL_DOWN)
ledRojo = Pin(2, Pin.OUT)
ledVerde = Pin(3, Pin.OUT)
ledAzul = Pin(4, Pin.OUT)
ledEmer = Pin(5, Pin.OUT)
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=100000)
I2C_ADDR = i2c.scan()[0] 
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)
lcd.putstr("Iniciando...")

modo_emergencia = False
count = 0

def interrupcion(Pin):
    global modo_emergencia
    modo_emergencia = not modo_emergencia
    ledEmer.value(modo_emergencia)

pulsador.irq(trigger=Pin.IRQ_RISING, handler=interrupcion)

def angle_pot():
    valorPot = pot.read_u16()  # entre 0 y 65535
    angulo = (valorPot / 65535) * 180
    return angulo

def angle_servo(angle):
    min_u16 = 1638   # 0.5 ms
    max_u16 = 8192   # 2.5 ms
    duty = int(min_u16 + (angle / 180) * (max_u16 - min_u16))
    servo.duty_u16(duty)

while True:
    angle_servo(0)
    if modo_emergencia:
        servo.duty_u16(0)
        ledAzul.off()
        ledRojo.off()
        ledVerde.off()
        lcd.move_to(0, 0)
        lcd.putstr(" EMERGENCIA     ")
        lcd.move_to(0, 1)
        lcd.putstr("                ")
        sleep(0.1)
        continue

    lectura_joy = joy.read_u16()
    angulo_paciente = angle_pot()
    print("Paciente:", angulo_paciente)

    if lectura_joy > 35557:
        count += 1
        if count > 180: count = 180
    elif lectura_joy < 1000:
        count -= 1
        if count < 0: count = 0

    angle_servo(count)
    print("Referencia:", count)
    diferencia = angulo_paciente - count

    if abs(diferencia) <= 10:
        ledVerde.on()
        ledAzul.off()
        ledRojo.off()
    elif diferencia > 10:
        ledRojo.on()
        ledAzul.off()
        ledVerde.off()
    else:
        ledAzul.on()
        ledRojo.off()
        ledVerde.off()

    # Actualización de pantalla LCD
    lcd.move_to(0, 0)
    lcd.putstr("Ref: {:3d}         ".format(count))
    lcd.move_to(0, 1)
    lcd.putstr("Pac: {:5.1f}       ".format(angulo_paciente))

    sleep(0.1)