import math
import struct
from typing import Dict, List, Tuple, Optional
from can_msg_builder import BROADCAST_MIX_CMD_ID, BROADCAST_TORQUE_CMD_ID


def parse_single_turn_state(data8: bytes) -> Dict:
    """
        解析电机状态2回复can帧
    """
    assert len(data8)==8
    temperature = struct.unpack("<b", data8[1:2])[0]
    iq_or_power = struct.unpack("<h", data8[2:4])[0]
    speed_dps = struct.unpack("<h", data8[4:6])[0]
    encoder = struct.unpack("<H", data8[6:8])[0]
    return dict(
        temperature_C=temperature,
        iq_or_power_raw=iq_or_power,
        speed_dps=speed_dps,
        rad=encoder / 65535 * 2 * math.pi,
    )

def parse_multi_turn_state(data8: bytes) -> Dict:
    """
        解析“读取多圈角度(0x92)”回复帧
        - DATA[0] = 0x92
        - DATA[1..7] = int64 motorAngle 的低 7 字节（little-endian）
        - 单位：0.01 deg / LSB
        """
    assert len(data8) == 8
    # 取低 7 字节
    low7 = data8[1:8]  # bytes length = 7
    # 符号扩展：看第7字节(低7字节中的最高字节)的最高位是否为1
    # 如果为1，说明负数，需要补 0xFF；否则补 0x00
    sign_ext = 0xFF if (low7[6] & 0x80) else 0x00
    raw8 = low7 + bytes([sign_ext])  # 补成 8 字节 little-endian 的 int64

    motor_angle_raw = struct.unpack("<q", raw8)[0]  # int64
    motor_angle_deg = math.radians(motor_angle_raw * 0.01)

    return dict(
        rad=motor_angle_deg,  # 单位：0.01deg/LSB
    )