"""
Test script to examine raw status update packets from the BBD202 controller.

This script:
1. Connects to the controller at a low level
2. Sends HW_START_UPDATEMSGS to enable automatic status updates
3. Captures and displays raw packets to verify the format matches our parsing
"""

import sys
import time
import struct
from hardware.bbd202 import MotionController, MsgId


def hexdump(data: bytes, prefix: str = "") -> str:
    """Format bytes as hex dump."""
    hex_str = " ".join(f"{b:02X}" for b in data)
    ascii_str = "".join(chr(b) if 32 <= b < 127 else "." for b in data)
    return f"{prefix}{hex_str}  |{ascii_str}|"


def decode_status_update(data: bytes) -> dict:
    """Decode a MOT_GET_USTATUSUPDATE message data payload."""
    if len(data) < 14:
        return {"error": f"Data too short: {len(data)} bytes, expected 14"}

    chan_ident = struct.unpack('<H', data[0:2])[0]
    position_counts = struct.unpack('<i', data[2:6])[0]
    velocity_counts = struct.unpack('<H', data[6:8])[0]
    motor_current = struct.unpack('<H', data[8:10])[0]
    status_bits = struct.unpack('<I', data[10:14])[0]

    # Convert to physical units
    ENCODER_COUNTS_PER_MM = 20000
    position_mm = position_counts / ENCODER_COUNTS_PER_MM

    return {
        "chan_ident": chan_ident,
        "position_counts": position_counts,
        "position_mm": position_mm,
        "velocity_counts": velocity_counts,
        "motor_current": motor_current,
        "status_bits": status_bits,
        "status_hex": f"0x{status_bits:08X}"
    }


def main():
    print("=" * 70)
    print("BBD202 Status Update Packet Test")
    print("=" * 70)

    mc = None
    try:
        # Connect to controller WITHOUT enabling updates yet
        print("\nConnecting to BBD202 controller...")
        mc = MotionController()
        mc.connect(enable_updates=False)  # Don't auto-enable updates
        print("Connected!")

        # Get hardware info
        try:
            hw_info = mc.get_hw_info()
            print(f"Hardware: {hw_info['model']} (S/N: {hw_info['serial_number']})")
            print(f"Firmware: {hw_info['firmware_version']}")
        except Exception as e:
            print(f"Could not get hardware info: {e}")

        # Clear any pending messages
        print("\nClearing RX queue...")
        msgs = mc.get_all_messages()
        print(f"Cleared {len(msgs)} pending messages")

        # Now enable status updates
        print("\n" + "-" * 70)
        print("Sending HW_START_UPDATEMSGS to enable automatic status updates...")
        print("-" * 70)
        mc.start_update_messages()

        # Wait a moment for updates to start arriving
        time.sleep(0.5)

        # Collect messages for a few seconds
        print("\nCollecting status update packets for 3 seconds...")
        print("(Looking for MOT_GET_USTATUSUPDATE = 0x0491)")
        print()

        start_time = time.time()
        update_count = 0
        other_count = 0

        while time.time() - start_time < 3.0:
            msg = mc.get_message(timeout=0.1)
            if msg:
                elapsed = time.time() - start_time

                if msg.msg_id == MsgId.MOT_GET_USTATUSUPDATE:
                    update_count += 1
                    source_name = "X-axis" if msg.source == 0x21 else "Y-axis" if msg.source == 0x22 else f"0x{msg.source:02X}"

                    print(f"[{elapsed:5.2f}s] MOT_GET_USTATUSUPDATE from {source_name}")
                    print(f"         Raw ({len(msg.raw)} bytes): {hexdump(msg.raw)}")
                    print(f"         Data ({len(msg.data)} bytes): {hexdump(msg.data)}")

                    decoded = decode_status_update(msg.data)
                    if "error" in decoded:
                        print(f"         DECODE ERROR: {decoded['error']}")
                    else:
                        print(f"         Decoded: chan={decoded['chan_ident']}, "
                              f"pos={decoded['position_mm']:.4f}mm ({decoded['position_counts']} counts), "
                              f"vel={decoded['velocity_counts']}, cur={decoded['motor_current']}, "
                              f"status={decoded['status_hex']}")
                    print()
                else:
                    other_count += 1
                    msg_name = MsgId(msg.msg_id).name if msg.msg_id in [m.value for m in MsgId] else f"0x{msg.msg_id:04X}"
                    print(f"[{elapsed:5.2f}s] Other message: {msg_name} from 0x{msg.source:02X}")
                    print(f"         Raw: {hexdump(msg.raw)}")
                    print()

        print("-" * 70)
        print(f"Summary: Received {update_count} status updates, {other_count} other messages")
        print("-" * 70)

        if update_count == 0:
            print("\n*** WARNING: No status updates received! ***")
            print("Possible causes:")
            print("  1. HW_START_UPDATEMSGS not being processed")
            print("  2. Controller firmware doesn't support automatic updates")
            print("  3. Updates are being sent but not parsed correctly")
            print("\nTrying to manually request a status update...")

            # Try requesting status update manually
            mc.request_status_update(mc.DEST_X_AXIS)
            mc.request_status_update(mc.DEST_Y_AXIS)
            time.sleep(0.5)

            msgs = mc.get_all_messages()
            print(f"\nReceived {len(msgs)} messages after manual request:")
            for msg in msgs:
                msg_name = MsgId(msg.msg_id).name if msg.msg_id in [m.value for m in MsgId] else f"0x{msg.msg_id:04X}"
                print(f"  {msg_name} from 0x{msg.source:02X}: {hexdump(msg.raw)}")

        # Also check cached positions
        print("\n" + "-" * 70)
        print("Cached positions in driver:")
        print("-" * 70)
        print(f"  X position: {mc.get_stage_position_x()}")
        print(f"  Y position: {mc.get_stage_position_y()}")
        print(f"  X encoder: {mc.encoder_count_x}")
        print(f"  Y encoder: {mc.encoder_count_y}")

        return 0

    except KeyboardInterrupt:
        print("\n\nTest interrupted by user")
        return 1
    except Exception as e:
        print(f"\n*** ERROR: {e}")
        import traceback
        traceback.print_exc()
        return 1
    finally:
        if mc is not None:
            print("\nDisconnecting from controller...")
            mc.disconnect()
            print("Disconnected")


if __name__ == "__main__":
    sys.exit(main())