# ============================================================
#  main.py — Integración final completa
#  Motor Controller: Pico W2 + TB6600 + NEMA 23
#  v1.0 — Pasos 1 al 8 integrados
# ============================================================

import machine
import time
import sys
import config
import storage
from state_machine import StateMachine
from stepper import Stepper
from watchdog import Watchdog


# ═══════════════════════════════════════════════════════════════
#  DIAGNÓSTICO INICIAL — corre antes de init del hardware
# ═══════════════════════════════════════════════════════════════
def run_diagnostics(i2c) -> bool:
    """
    Verifica que el LCD responda en el bus I2C.
    Retorna True si todo OK, False si hay problemas.
    """
    print("=" * 40)
    print(f"  {config.PROJECT_NAME} {config.FW_VERSION}")
    print(f"  MicroPython {sys.version}")
    print("=" * 40)

    devices = i2c.scan()
    print(f"[I2C] Dispositivos encontrados: {[hex(d) for d in devices]}")

    if config.LCD_I2C_ADDR not in devices:
        print(f"[ERROR] LCD no responde en 0x{config.LCD_I2C_ADDR:02X}")
        print("[HINT] Probar con 0x3F — cambiar LCD_I2C_ADDR en config.py")
        # Parpadear LED onboard para señalizar error
        led = machine.Pin("LED", machine.Pin.OUT)
        for _ in range(10):
            led.toggle()
            time.sleep_ms(200)
        return False

    print(f"[OK] LCD en 0x{config.LCD_I2C_ADDR:02X}")
    return True


# ═══════════════════════════════════════════════════════════════
#  INICIALIZACIÓN DE HARDWARE
# ═══════════════════════════════════════════════════════════════

# ── LED onboard (indicador de estado) ────────────────────────
led = machine.Pin("LED", machine.Pin.OUT)
led.value(1)   # encendido = sistema iniciando

# ── I2C para LCD ─────────────────────────────────────────────
i2c = machine.I2C(
    0,
    sda=machine.Pin(config.I2C_SDA),
    scl=machine.Pin(config.I2C_SCL),
    freq=config.I2C_FREQ
)

# ── Diagnóstico — detiene el arranque si el LCD no responde ──
if not run_diagnostics(i2c):
    print("[FATAL] Revisar conexiones y reiniciar")
    sys.exit()

# ── Pines del TB6600 ─────────────────────────────────────────
step_pin = machine.Pin(config.STEP_PIN, machine.Pin.OUT)
dir_pin  = machine.Pin(config.DIR_PIN,  machine.Pin.OUT)
ena_pin  = machine.Pin(config.ENA_PIN,  machine.Pin.OUT)
ena_pin.value(1)   # HIGH = deshabilitado hasta que el usuario actúe

# ── Estado global compartido ─────────────────────────────────
state = {
    "mode"         : config.MODE_VELOCITY,
    "max_speed"    : config.DEFAULT_MAX_SPEED,
    "accel"        : config.DEFAULT_ACCEL,
    "microstep"    : config.DEFAULT_MICROSTEP,
    "target_speed" : 0.0,
    "position_deg" : 0.0,
    "motor_running": False,
}

# ── Cargar parámetros desde flash ────────────────────────────
ok = storage.load(state)
print(f"[Storage] {'Parámetros cargados' if ok else 'Usando defaults'}")
print(f"  max_speed  = {state['max_speed']} p/s")
print(f"  accel      = {state['accel']} p/s2")
print(f"  microstep  = 1/{state['microstep']}")

# ── Objetos principales ───────────────────────────────────────
motor = Stepper(step_pin, dir_pin, ena_pin, state)
sm    = StateMachine(i2c, state)
wdt   = Watchdog(timeout_ms=5000)


# ═══════════════════════════════════════════════════════════════
#  CALLBACKS — conectan la UI con el motor
# ═══════════════════════════════════════════════════════════════

def on_speed_change(speed: float):
    """Se llama cuando UP/DOWN cambian la velocidad objetivo."""
    if state["motor_running"]:
        motor.run(speed)


def on_motor_toggle():
    """Se llama cuando ENTER arranca o para el motor."""
    motor.toggle()
    # LED sigue el estado del motor
    led.value(1 if state["motor_running"] else 0)


def on_position_step(direction: int):
    """Se llama cuando UP/DOWN mueven el motor en modo posición."""
    motor.step(direction)


def on_position_reset():
    """Se llama cuando ENTER resetea la posición a 0°."""
    motor.reset_position()


def on_params_saved():
    """Se llama al confirmar un cambio en el menú."""
    saved = storage.save(state)
    motor.update_params()

    # Ajustar target_speed si supera el nuevo máximo
    if state["target_speed"] > state["max_speed"]:
        state["target_speed"] = state["max_speed"]
        if state["motor_running"]:
            motor.run(state["target_speed"])

    status = "OK" if saved else "ERROR"
    print(f"[Params] Guardado {status} — "
          f"speed={state['max_speed']} "
          f"accel={state['accel']} "
          f"microstep={state['microstep']}")


# ── Asignar callbacks a la máquina de estados ─────────────────
sm.on_speed_change   = on_speed_change
sm.on_motor_toggle   = on_motor_toggle
sm.on_position_step  = on_position_step
sm.on_position_reset = on_position_reset
sm.on_params_saved   = on_params_saved


# ═══════════════════════════════════════════════════════════════
#  LOOP PRINCIPAL
# ═══════════════════════════════════════════════════════════════
print(f"\n[OK] {config.PROJECT_NAME} {config.FW_VERSION} — Sistema listo\n")
led.value(0)   # LED apagado = sistema listo, motor parado

_last_display_ms = 0
_DISPLAY_REFRESH_MS = 200   # refresco máximo del display (ms)

try:
    while True:
        now = time.ticks_ms()

        # ── Actualizar botones y lógica de modos ──────────────
        sm.update()

        # ── Refresco periódico del display ────────────────────
        # Actualiza velocidad/posición aunque no haya pulsación
        if time.ticks_diff(now, _last_display_ms) >= _DISPLAY_REFRESH_MS:
            sm.force_refresh()
            _last_display_ms = now

        # ── LED parpadea mientras el motor corre ──────────────
        if state["motor_running"]:
            led.value(1 if (now // 500) % 2 == 0 else 0)
        else:
            led.value(0)

        # ── Alimentar watchdog ────────────────────────────────
        wdt.feed()

        time.sleep_ms(10)

except Exception as e:
    # Error inesperado — parar motor y mostrar en display
    print(f"[FATAL] Excepción: {e}")
    motor.emergency_stop()
    led.value(1)
    time.sleep_ms(2000)
    machine.reset()   # reinicio automático