"""CANalyst-II + MS3008 广播模式简单手动测试。"""

import argparse
import struct
import time

import can

CAN_CHANNEL = 0  # CANalyst-II 面板 CAN1
CAN_BITRATE = 1000000
CAN_DEVICE = 0

CONTROL_FRAME_ID = 0x280
COMMAND_FRAME_ID = 0x288
READ_STATUS_2 = 0x9C
MOTOR_ON_COMMAND = 0x88
STOP_COMMAND = 0x81


def open_can() -> can.BusABC:
    """打开 CANalyst-II：channel=0 对应面板 CAN1。"""
    return can.Bus(
        interface="canalystii",
        channel=CAN_CHANNEL,
        bitrate=CAN_BITRATE,
        device=CAN_DEVICE,
    )


def read_status(bus: can.BusABC, timeout: float = 0.5) -> dict[int, dict[str, int]]:
    """广播请求 ID 1、2 的状态2（0x9C）。"""
    data = bytes([READ_STATUS_2, 0, READ_STATUS_2, 0, 0, 0, 0, 0])
    message = can.Message(
        arbitration_id=COMMAND_FRAME_ID,
        is_extended_id=False,
        data=data,
    )
    bus.send(message)
    print(f"TX  ID=0x{COMMAND_FRAME_ID:03X}  DATA={data.hex(' ').upper()}")

    reports: dict[int, dict[str, int]] = {}
    deadline = time.monotonic() + timeout
    while len(reports) < 2:
        remaining = deadline - time.monotonic()
        if remaining <= 0:
            break

        reply = bus.recv(timeout=remaining)
        print(reply)
        if reply is None:
            break

        motor_id = reply.arbitration_id - 0x140
        if motor_id not in (1, 2) or len(reply.data) != 8 or reply.data[0] != READ_STATUS_2:
            continue

        temperature = struct.unpack_from("<b", reply.data, 1)[0]
        power_raw, speed_dps, encoder_raw = struct.unpack_from("<hhH", reply.data, 2)
        reports[motor_id] = {
            "temperature_c": temperature,
            "power_raw": power_raw,
            "speed_dps": speed_dps,
            "encoder_raw": encoder_raw,
        }
        print(
            f"RX  ID=0x{reply.arbitration_id:03X}  "
            f"DATA={bytes(reply.data).hex(' ').upper()}  "
            f"motor={motor_id} temp={temperature}C power={power_raw} "
            f"speed={speed_dps}dps encoder={encoder_raw}"
        )

    missing = sorted({1, 2} - reports.keys())
    if missing:
        print(f"未收到电机 {missing} 的状态回包")
    return reports


def torque_control(
    bus: can.BusABC,
    motor1: int,
    motor2: int,
    frequency_hz: float = 10.0,
) -> None:
    """
    广播发送 ID 1、2 的控制量。

    MS3008 中该值实际是 -850..850 的开环 powerControl raw，不是 N·m。
    """
    if not -850 <= motor1 <= 850 or not -850 <= motor2 <= 850:
        raise ValueError("MS3008 控制量必须在 -850..850")

    enable_data = bytes([MOTOR_ON_COMMAND, 0, MOTOR_ON_COMMAND, 0, 0, 0, 0, 0])
    bus.send(
        can.Message(
            arbitration_id=COMMAND_FRAME_ID,
            is_extended_id=False,
            data=enable_data,
        )
    )
    print(f"TX  ID=0x{COMMAND_FRAME_ID:03X}  DATA={enable_data.hex(' ').upper()}")
    time.sleep(0.02)

    control_data = struct.pack("<hhhh", motor1, motor2, 0, 0)
    control_message = can.Message(
        arbitration_id=CONTROL_FRAME_ID,
        is_extended_id=False,
        data=control_data,
    )
    period = 1.0 / frequency_hz
    next_send = time.monotonic()
    count = 0
    try:
        while True:
            bus.send(control_message)
            count += 1
            if count % 100 == 0:
                print(
                    f"TX  ID=0x{CONTROL_FRAME_ID:03X}  "
                    f"DATA={control_data.hex(' ').upper()}  count={count}"
                )
            next_send += period
            now = time.monotonic()
            if next_send > now:
                time.sleep(next_send - now)
            else:
                next_send = now
    finally:
        stop(bus)


def stop(bus: can.BusABC) -> None:
    """连续两轮广播停止 ID 1、2（零输出 + 0x81）。"""
    zero_data = bytes(8)
    zero_message = can.Message(
        arbitration_id=CONTROL_FRAME_ID,
        is_extended_id=False,
        data=zero_data,
    )
    data = bytes([STOP_COMMAND, 0, STOP_COMMAND, 0, 0, 0, 0, 0])
    stop_message = can.Message(
        arbitration_id=COMMAND_FRAME_ID,
        is_extended_id=False,
        data=data,
    )

    for attempt in range(2):
        bus.send(zero_message)
        print(f"TX  ID=0x{CONTROL_FRAME_ID:03X}  DATA={zero_data.hex(' ').upper()}")
        bus.send(stop_message, timeout=0.1)
        print(f"TX  ID=0x{COMMAND_FRAME_ID:03X}  DATA={data.hex(' ').upper()}")
        if attempt == 0:
            time.sleep(0.02)


def main() -> None:
    parser = argparse.ArgumentParser()
    commands = parser.add_subparsers(dest="command", required=True)
    commands.add_parser("status", help="读取 ID 1、2 状态")

    torque_parser = commands.add_parser("torque", help="发送 ID 1、2 控制量")
    torque_parser.add_argument("motor1", type=int, help="ID 1 控制量")
    torque_parser.add_argument("motor2", type=int, help="ID 2 控制量")

    commands.add_parser("stop", help="停止 ID 1、2")
    args = parser.parse_args()

    bus = open_can()
    print(bus)
    try:
        if args.command == "status":
            read_status(bus)
        elif args.command == "torque":
            try:
                torque_control(bus, args.motor1, args.motor2)
            except KeyboardInterrupt:
                print("已停止")
        else:
            stop(bus)
    finally:
        bus.shutdown()


if __name__ == "__main__":
    main()