"""
MicroMovimiento — Control de motor con joystick.
RP2040-Zero: GP2..5 -> IN1..4, GP26/ADC0 <- VRx.
Motor: fuente 5V separada; GND comun; 5V de placa SIN CONECTAR.
Confirmar ULN2003A y las cuatro entradas del conector antes de alimentar.
"""
from machine import Pin, ADC
import time
from control import SECUENCIA, objetivo, rampa

# Guardia de entrega: cambiar a True SOLO tras verificaciones de GUIA.html.
HARDWARE_CONFIRMADO = False

def ejecutar():
    salidas = [Pin(n, Pin.OUT, value=0) for n in (2,3,4,5)]
    def apagar():
        for p in salidas:
            p.value(0)
    if not HARDWARE_CONFIRMADO:
        print("MicroMovimiento bloqueado: revisar hardware y cambiar bandera.")
        apagar()
        return
    adc = ADC(Pin(26))
    print("Soltar joystick al centro durante 2 segundos.")
    try:
        muestras = []
        for _ in range(200):
            muestras.append(adc.read_u16())
            time.sleep_ms(10)
        centro = sum(muestras)//len(muestras)
        if not 24000 <= centro <= 41000 or max(muestras)-min(muestras)>6000:
            raise ValueError("Centro inestable/fuera de rango. Revisar 3V3, GND, VRx.")
        print("Centro:", centro, "Ctrl+C detiene y libera bobinas.")
        filtrado = centro
        velocidad = 0.0
        indice = 0
        previo = time.ticks_us()
        ultimo_paso = previo
        while True:
            ahora = time.ticks_us()
            dt = time.ticks_diff(ahora,previo)/1000000.0
            previo = ahora
            lectura = adc.read_u16()
            # Filtro sencillo, actualizado cada ~5ms.
            filtrado = (3*filtrado+lectura)//4
            destino = objetivo(filtrado,centro)
            if destino == 0:
                # Centro detiene inmediatamente y libera el eje.
                velocidad = 0.0
                apagar()
                ultimo_paso = ahora
            else:
                velocidad = rampa(velocidad,destino,dt)
                if abs(velocidad) >= 1:
                    intervalo = int(1000000/abs(velocidad))
                    if time.ticks_diff(ahora,ultimo_paso) >= intervalo:
                        indice = (indice+(1 if velocidad>0 else -1))%8
                        for p,valor in zip(salidas,SECUENCIA[indice]):
                            p.value(valor)
                        ultimo_paso = ahora  # No recuperar pasos atrasados en rafagas.
                else:
                    apagar()
                    ultimo_paso = ahora
            time.sleep_ms(1)
    finally:
        apagar()  # Incluye Ctrl+C y excepciones.

ejecutar()