#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import argparse
import math
import struct
import can


CMD_READ_MULTITURN_ANGLE = 0x92


def arb_id_single(motor_id: int) -> int:
    # 单电机命令/回包 arbitration id
    return 0x140 + int(motor_id)


def build_read_multiturn_angle(motor_id: int) -> can.Message:
    """
    读取多圈角度命令：
    data[0]=0x92，其余填0
    """
    data = [CMD_READ_MULTITURN_ANGLE, 0, 0, 0, 0, 0, 0, 0]
    return can.Message(arbitration_id=arb_id_single(motor_id), is_extended_id=False, data=data)


def sign_extend(value: int, bits: int) -> int:
    """
    对 value 做符号扩展，使其成为 Python int（有符号）
    """
    sign_bit = 1 << (bits - 1)
    mask = (1 << bits) - 1
    value &= mask
    return (value ^ sign_bit) - sign_bit


def parse_multiturn_angle_reply_int56(msg: can.Message) -> int:
    """
    兼容你贴的文档：DATA[1..7] 是角度的低->高字节（共7字节）
    => 解析为有符号 int56
    单位：0.01 deg / LSB
    """
    d = bytes(msg.data)
    if len(d) != 8 or d[0] != CMD_READ_MULTITURN_ANGLE:
        raise ValueError("not 0x92 reply")
    raw_u56 = int.from_bytes(d[1:8], byteorder="little", signed=False)  # 7 bytes
    raw_s56 = sign_extend(raw_u56, 56)
    return raw_s56


def parse_multiturn_angle_reply_int64(msg: can.Message) -> int:
    """
    如果你确认实际回的是 8 字节 motorAngle（int64）：
    常见做法是 DATA[0..7] 都是 motorAngle 的 8 字节（但会与“命令字节=0x92”冲突）
    或者命令字节在别处。
    这个函数留作你确认后切换用。
    """
    d = bytes(msg.data)
    # 这里假设 d[0] 仍是 0x92，则 motorAngle 应该在 d[1:9] 不存在
    # 所以只有当你的实际报文不是“d[0]=0x92”时才用它
    raise NotImplementedError("Need exact 8-byte layout confirmation.")


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--interface", default="canalystii", help="python-can interface: canalystii / socketcan / ...")
    ap.add_argument("--channel", default="0", help="channel: canalystii 0/1, socketcan can0/can1 ...")
    ap.add_argument("--bitrate", type=int, default=1000000)
    ap.add_argument("--motor-id", type=int, default=2)
    ap.add_argument("--timeout", type=float, default=0.2, help="seconds")
    ap.add_argument("--repeat", type=int, default=3, help="query times")
    args = ap.parse_args()

    motor_id = args.motor_id
    rx_arb = arb_id_single(motor_id)

    bus = can.Bus(interface=args.interface, channel=args.channel, bitrate=args.bitrate)

    # 只收这个电机的回包（降低噪声）
    try:
        bus.set_filters([{"can_id": rx_arb, "can_mask": 0x7FF, "extended": False}])
    except Exception:
        pass

    try:
        for i in range(args.repeat):
            # 发送 0x92
            bus.send(build_read_multiturn_angle(motor_id))

            # 等回包
            msg = bus.recv(timeout=args.timeout)
            if msg is None:
                print(f"[M{motor_id}] timeout: no reply")
                continue
            if msg.arbitration_id != rx_arb or len(msg.data) != 8:
                print(f"[M{motor_id}] unexpected frame: arb=0x{msg.arbitration_id:X} data={list(msg.data)}")
                continue

            # 解析（按你贴的文档：int56 in DATA[1..7]）
            motor_angle_raw = parse_multiturn_angle_reply_int56(msg)

            # 单位换算：0.01°/LSB
            angle_deg = motor_angle_raw * 0.01
            angle_rad = angle_deg * math.pi / 180.0

            print(
                f"[M{motor_id}] motorAngle_raw={motor_angle_raw}  "
                f"angle_deg={angle_deg:+.4f}°  angle_rad={angle_rad:+.6f} rad"
            )

    finally:
        bus.shutdown()


if __name__ == "__main__":
    main()