from machine import Pin
from time import sleep_ms, ticks_ms, ticks_diff
from servo import Servo  # usa tu servo.py

# --- CONFIGURACIÓN DE PINES ---
# Cambia los GPIO si lo deseas, evitando 0/2/15 por temas de arranque.
SERVO1_PIN = 18
SERVO2_PIN = 19
BTN1_PIN   = 32   # botón para servo 1 (pull-up interno, activo en LOW)
BTN2_PIN   = 33   # botón para servo 2 (pull-up interno, activo en LOW)

# --- OBJETOS SERVO ---
s1 = Servo(Pin(SERVO1_PIN))  # por defecto: 50 Hz, 600–2400 µs, 180°
s2 = Servo(Pin(SERVO2_PIN))

# Posiciones objetivo para "toggle"
POS_MIN = 0
POS_MAX = 180

# Estado actual
pos1 = POS_MIN
pos2 = POS_MIN

# --- BOTONES con pull-up + antirrebote ---
btn1 = Pin(BTN1_PIN, Pin.IN, Pin.PULL_UP)
btn2 = Pin(BTN2_PIN, Pin.IN, Pin.PULL_UP)

DEBOUNCE_MS = 80
last1 = ticks_ms()
last2 = ticks_ms()

def set_servo_angle(servo, angle):
    # usa la API de tu librería: write_angle(degrees=...)
    servo.write_angle(degrees=angle)

# Llevar ambos a posición inicial
set_servo_angle(s1, pos1)
set_servo_angle(s2, pos2)

print("Listo. Pulsa BTN1 para Servo1 y BTN2 para Servo2.")

# Detección por flanco (de HIGH->LOW), con antirrebote por tiempo
prev_b1 = btn1.value()
prev_b2 = btn2.value()

while True:
    # --- Botón 1 ---
    v1 = btn1.value()
    if prev_b1 == 1 and v1 == 0:  # flanco descendente
        if ticks_diff(ticks_ms(), last1) > DEBOUNCE_MS:
            pos1 = POS_MAX if pos1 == POS_MIN else POS_MIN
            set_servo_angle(s1, pos1)
            last1 = ticks_ms()
    prev_b1 = v1

    # --- Botón 2 ---
    v2 = btn2.value()
    if prev_b2 == 1 and v2 == 0:  # flanco descendente
        if ticks_diff(ticks_ms(), last2) > DEBOUNCE_MS:
            pos2 = POS_MAX if pos2 == POS_MIN else POS_MIN
            set_servo_angle(s2, pos2)
            last2 = ticks_ms()
    prev_b2 = v2

    sleep_ms(10)