from machine import Pin, ADC, PWM
import time
# ---------------------- CONFIGURACIÓN ------------------------
# Pines (ajústalos a tu hardware real)
PIN_SERVO_BASE = 14       # Servo 1: Rotación base
PIN_SERVO_BRAZO = 27      # Servo 2: Elevación brazo
PIN_POT_BASE   = 32       # Potenciómetro 1 (12 bits)
PIN_POT_BRAZO  = 33       # Potenciómetro 2 (10 bits)
PIN_BTN_HOME   = 25       # Pulsador HOME
PIN_BTN_RUTINA = 26       # Pulsador RUTINA

PIN_LED_VERDE = 18        # LED control manual
PIN_LED_ROJO  = 19        # LED retorno automático
PIN_BUZZER    = 23        # Buzzer activo

# Servo (50 Hz)
PWM_FREQ = 50
SERVO_US_MIN = 500
SERVO_US_MAX = 2500
SERVO_MIN_DEG = 0
SERVO_MAX_DEG = 180

# Posición inicial (HOME)
HOME_BASE = 90
HOME_BRAZO = 90

# Antirrebote
DEBOUNCE_MS = 150

# Resoluciones de ADC
ADC_BASE_RES   = 4095   # 12 bits
ADC_BRAZO_RES  = 1023   # 10 bits

# ---------------------- CLASE SERVO -------------------------
class Servo:
    def __init__(self, pin, freq=PWM_FREQ):
        self.pwm = PWM(Pin(pin), freq=freq, duty=0)
        self.period_us = int(1_000_000 / freq)

    def angle_to_us(self, deg):
        span_deg = SERVO_MAX_DEG - SERVO_MIN_DEG
        span_us  = SERVO_US_MAX - SERVO_US_MIN
        us = SERVO_US_MIN + (deg - SERVO_MIN_DEG) * span_us / span_deg
        return int(us)

    def us_to_duty(self, us):
        duty = int((us / self.period_us) * 1023)
        return max(0, min(1023, duty))

    def write_angle(self, deg):
        us = self.angle_to_us(deg)
        duty = self.us_to_duty(us)
        self.pwm.duty(duty)

# ---------------------- FUNCIONES AUX ------------------------
def map_range(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

def clamp(v, vmin, vmax):
    return vmin if v < vmin else vmax if v > vmax else v

def estado_manual():
    led_verde.value(1)
    led_rojo.value(0)
    buzzer.value(0)

def estado_home():
    led_verde.value(0)
    led_rojo.value(1)
    buzzer.value(1)

def estado_idle():
    led_verde.value(0)
    led_rojo.value(0)
    buzzer.value(0)

# ---------------------- INICIALIZACIÓN ------------------------
servo_base = Servo(PIN_SERVO_BASE)
servo_brazo = Servo(PIN_SERVO_BRAZO)

# ADC configuración
adc_base = ADC(Pin(PIN_POT_BASE))
adc_brazo = ADC(Pin(PIN_POT_BRAZO))
adc_base.atten(ADC.ATTN_11DB)   # rango 0-3.3V
adc_brazo.atten(ADC.ATTN_11DB)
adc_base.width(ADC.WIDTH_12BIT) # 12 bits
adc_brazo.width(ADC.WIDTH_10BIT)# 10 bits

# Pulsadores
btn_home   = Pin(PIN_BTN_HOME, Pin.IN, Pin.PULL_UP)
btn_rutina = Pin(PIN_BTN_RUTINA, Pin.IN, Pin.PULL_UP)

# LEDs y buzzer
led_verde = Pin(PIN_LED_VERDE, Pin.OUT)
led_rojo  = Pin(PIN_LED_ROJO, Pin.OUT)
buzzer    = Pin(PIN_BUZZER, Pin.OUT)

# ---------------------- FLAGS / ESTADOS ----------------------
modo_manual = True
flag_home   = False
flag_rutina = False
last_home_ms   = 0
last_rutina_ms = 0

# ---------------------- ISR BOTONES --------------------------
def irq_home(pin):
    global flag_home, last_home_ms
    now = time.ticks_ms()
    if time.ticks_diff(now, last_home_ms) > DEBOUNCE_MS:
        flag_home = True
        last_home_ms = now

def irq_rutina(pin):
    global flag_rutina, last_rutina_ms
    now = time.ticks_ms()
    if time.ticks_diff(now, last_rutina_ms) > DEBOUNCE_MS:
        flag_rutina = True
        last_rutina_ms = now

btn_home.irq(trigger=Pin.IRQ_FALLING, handler=irq_home)
btn_rutina.irq(trigger=Pin.IRQ_FALLING, handler=irq_rutina)

# ---------------------- RUTINA -------------------------------
RUTINA = [
    (60,  60, 500),
    (120, 90, 500),
    (150, 50, 500),
    (90,  90, 500),
]

def ejecutar_rutina(prev_base, prev_brazo):
    for (b, a, hold) in RUTINA:
        servo_base.write_angle(b)
        servo_brazo.write_angle(a)
        time.sleep_ms(hold)
    # Regresar a la posición previa
    servo_base.write_angle(prev_base)
    servo_brazo.write_angle(prev_brazo)

# ---------------------- LOOP PRINCIPAL -----------------------
servo_base.write_angle(HOME_BASE)
servo_brazo.write_angle(HOME_BRAZO)

cur_base = HOME_BASE
cur_brazo = HOME_BRAZO

estado_manual()

while True:
    # ----- HOME -----
    if flag_home:
        flag_home = False
        modo_manual = False
        estado_home()
        servo_base.write_angle(HOME_BASE)
        servo_brazo.write_angle(HOME_BRAZO)
        cur_base, cur_brazo = HOME_BASE, HOME_BRAZO
        estado_manual()
        modo_manual = True

    # ----- RUTINA -----
    elif flag_rutina:
        flag_rutina = False
        modo_manual = False
        estado_idle()  # LEDs apagados, buzzer off en rutina
        ejecutar_rutina(cur_base, cur_brazo)
        modo_manual = True
        estado_manual()

    # ----- MANUAL -----
    if modo_manual:
        # Lectura de potenciómetros
        val_base = adc_base.read()   # 12 bits
        val_brazo = adc_brazo.read() # 10 bits

        # Escalado a 0-180 grados
        deg_base = int(map_range(val_base, 0, ADC_BASE_RES, 0, 180))
        deg_brazo = int(map_range(val_brazo, 0, ADC_BRAZO_RES, 0, 180))

        deg_base = clamp(deg_base, 0, 180)
        deg_brazo = clamp(deg_brazo, 0, 180)

        # Actualizar servos
        servo_base.write_angle(deg_base)
        servo_brazo.write_angle(deg_brazo)

        cur_base, cur_brazo = deg_base, deg_brazo

        time.sleep_ms(50)