from machine import Pin, PWM
import time

# Configuración de pines
servo_pin = Pin(2, Pin.OUT)
led_rojo = Pin(4, Pin.OUT)
led_verde = Pin(5, Pin.OUT)
boton_izq = Pin(18, Pin.IN, Pin.PULL_UP)  # Botón para girar izquierda
boton_der = Pin(19, Pin.IN, Pin.PULL_UP)  # Botón para girar derecha

# Configuración del servomotor (PWM)
servo = PWM(servo_pin)
servo.freq(50)  # Frecuencia de 50Hz para servos

# Variables de control
posicion_actual = 90  # Posición central del servo (90 grados)
servo_activo = False
ultimo_boton_izq = 1
ultimo_boton_der = 1

def mover_servo(angulo):
    """Mueve el servo al ángulo especificado (0-180 grados)"""
    # Conversión de ángulos a duty cycle
    # 0 grados = ~25 duty, 90 grados = ~75 duty, 180 grados = ~125 duty
    duty = int(25 + (angulo / 180) * 100)
    servo.duty(duty)

def mover_servo_lento(angulo_destino, pasos=100, delay=0.2):
    """Mueve el servo lentamente al ángulo destino"""
    global posicion_actual
    
    # Calcular la diferencia y el incremento por paso
    diferencia = angulo_destino - posicion_actual
    incremento = diferencia / pasos
    
    # Mover gradualmente
    for i in range(pasos):
        posicion_actual += incremento
        mover_servo(int(posicion_actual))
        time.sleep(delay)
    
    # Asegurar posición final exacta
    posicion_actual = angulo_destino
    mover_servo(angulo_destino)

def apagar_leds():
    """Apaga todos los LEDs"""
    led_rojo.off()
    led_verde.off()

def detener_servo():
    """Detiene el servo y apaga LEDs"""
    global servo_activo
    servo_activo = False
    servo.deinit()  # Desactiva el PWM
    apagar_leds()
    print("Servo detenido")

def inicializar_servo():
    """Inicializa el servo en posición central"""
    global servo, posicion_actual
    servo = PWM(servo_pin)
    servo.freq(50)
    posicion_actual = 90  # Actualizar posición actual
    mover_servo(90)  # Posición central

print("Sistema iniciado - Presiona los botones para controlar el servo")
print("Mantén presionado cualquier botón para detener el servo")

# Bucle principal
while True:
    # Leer estado actual de los botones (invertido porque usamos PULL_UP)
    estado_izq = not boton_izq.value()
    estado_der = not boton_der.value()
    
    # Detectar si se presiona el botón izquierdo
    if estado_izq and not ultimo_boton_izq:
        if not servo_activo:
            print("Girando de 180° a 0° (izquierda)")
            led_rojo.on()  # Encender LED desde el inicio
            led_verde.off()
            servo = PWM(servo_pin)
            servo.freq(50)
            posicion_actual = 180  # Empezar desde la derecha
            mover_servo(180)  # Ir primero a 180°
            time.sleep(0.2)  # Pausa para estabilizar
            mover_servo_lento(0)  # Luego ir lentamente a 0°
            servo_activo = True
        else:
            print("Deteniendo servo (botón izquierdo mantenido)")
            detener_servo()
    
    # Detectar si se presiona el botón derecho
    elif estado_der and not ultimo_boton_der:
        if not servo_activo:
            print("Girando de 0° a 180° (derecha)")
            led_verde.on()  # Encender LED desde el inicio
            led_rojo.off()
            servo = PWM(servo_pin)
            servo.freq(50)
            posicion_actual = 0  # Empezar desde la izquierda
            mover_servo(0)  # Ir primero a 0°
            time.sleep(0.2)  # Pausa para estabilizar
            mover_servo_lento(180)  # Luego ir lentamente a 180°
            servo_activo = True
        else:
            print("Deteniendo servo (botón derecho mantenido)")
            detener_servo()
    
    # Detectar si se mantiene presionado cualquier botón para detener
    if (estado_izq or estado_der) and servo_activo:
        # Si el botón se mantiene presionado por más tiempo, detener
        time.sleep(0.1)  # Pequeña pausa para estabilidad
        if boton_izq.value() == 0 or boton_der.value() == 0:  # Aún presionado
            detener_servo()
    
    # Actualizar estados anteriores
    ultimo_boton_izq = estado_izq
    ultimo_boton_der = estado_der
    
    time.sleep(0.05)  # Pequeña pausa para evitar lecturas excesivas