#!/usr/bin/env python3
"""Incremental BOOT_8PSK debug tester for SkyWalker-1.

Sends debug boot modes (wValue=0x80..0x83) one at a time to isolate
which stage of the BCM4500 boot sequence hangs the FX2 firmware.

Usage:
    sudo python3 test_boot_debug.py          # run all debug stages
    sudo python3 test_boot_debug.py 0x82     # run only stage 0x82
"""

import usb.core
import usb.util
import sys
import time

BOOT_8PSK = 0x89

def find_device():
    dev = usb.core.find(idVendor=0x09C0, idProduct=0x0203)
    if not dev:
        print("Device not found!")
        sys.exit(1)
    return dev

def setup_device(dev):
    try:
        if dev.is_kernel_driver_active(0):
            dev.detach_kernel_driver(0)
    except Exception:
        pass
    try:
        dev.set_configuration()
    except usb.core.USBError:
        pass

def decode_stage(stage):
    names = {
        0x00: "NOT_STARTED",
        0x01: "GPIO_SETUP",
        0x02: "PWR_SETTLED",
        0x03: "I2C_PROBE",
        0x04: "INIT_BLK0",
        0x05: "INIT_BLK1",
        0x06: "INIT_BLK2",
        0xA1: "DEBUG_GPIO_OK",
        0xA2: "DEBUG_PROBE_OK",
        0xA3: "DEBUG_BLK0_OK",
        0xE3: "DEBUG_PROBE_FAIL",
        0xE4: "DEBUG_BLK0_FAIL",
        0xFF: "COMPLETE",
    }
    return names.get(stage, f"UNKNOWN(0x{stage:02X})")

def test_mode(dev, wval, label, timeout_ms=3000):
    """Send a debug boot mode and read 3-byte response."""
    print(f"\n{'─' * 50}")
    print(f"  Testing wValue=0x{wval:02X}: {label}")
    print(f"{'─' * 50}")

    t0 = time.monotonic()
    try:
        ret = dev.ctrl_transfer(0xC0, BOOT_8PSK, wval, 0, 3, timeout=timeout_ms)
    except usb.core.USBError as e:
        elapsed = (time.monotonic() - t0) * 1000
        print(f"  FAILED after {elapsed:.0f}ms: {e}")
        # Try to see if device is still alive
        try:
            dev.ctrl_transfer(0xC0, 0x92, 0, 0, 6, timeout=1000)
            print("  Device still responds to GET_FW_VERS")
        except:
            print("  Device is HUNG (no response to GET_FW_VERS)")
        return None
    elapsed = (time.monotonic() - t0) * 1000

    status = ret[0]
    stage = ret[1] if len(ret) > 1 else 0
    probe = ret[2] if len(ret) > 2 else 0

    print(f"  Response in {elapsed:.0f}ms:")
    print(f"    config_status: 0x{status:02X}")
    print(f"    boot_stage:    0x{stage:02X}  [{decode_stage(stage)}]")
    print(f"    probe_byte:    0x{probe:02X}")
    return ret

def main():
    dev = find_device()
    setup_device(dev)

    # Verify firmware is responding
    try:
        ret = dev.ctrl_transfer(0xC0, 0x92, 0, 0, 6, timeout=2000)
        major, minor, patch = ret[2], ret[1], ret[0]
        print(f"Firmware: v{major}.{minor:02d}.{patch}")
    except usb.core.USBError as e:
        print(f"GET_FW_VERS failed: {e}")
        print("Device may be hung. Try reloading firmware with fw_load.py.")
        sys.exit(1)

    ret = dev.ctrl_transfer(0xC0, 0x80, 0, 0, 1)
    print(f"Config: 0x{ret[0]:02X}")

    # Parse optional argument for single-stage testing
    single_stage = None
    if len(sys.argv) > 1:
        single_stage = int(sys.argv[1], 0)

    stages = [
        (0x80, "No-op: return current state only"),
        (0x81, "GPIO setup + power + delays (no I2C)"),
        (0x82, "GPIO + I2C bus reset + BCM4500 probe read"),
        (0x83, "GPIO + I2C probe + write init block 0"),
    ]

    for wval, label in stages:
        if single_stage is not None and wval != single_stage:
            continue
        result = test_mode(dev, wval, label)
        if result is None:
            print("\n*** STOPPING: device not responding ***")
            break

    print(f"\n{'=' * 50}")
    print("Debug complete.")

if __name__ == "__main__":
    main()