#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
瓴控电机 CAN 广播控制（V2.35）：
- 力矩/开环控制：帧ID 0x280，data[0..7] 为 4 个电机的 int16 torqueValue（低字节在前，小端）
- 混合命令：帧ID 0x288，data[0,2,4,6] 为 4 个电机 motorCmd（如 0x88 开启 / 0x81 停止 / 0x80 关闭）

依赖：
  pip install python-can

运行前准备（Linux 推荐 SocketCAN）：
  sudo ip link set can0 up type can bitrate 500000
  或者 1000000（看你驱动与设备设置）
"""

from __future__ import annotations
import time
from typing import Iterable, Tuple

import can


TORQUE_CMD_ID = 0x280
MIXED_CMD_ID  = 0x288

# 混合命令 motorCmd（来自文档）
CMD_READ_STATUS1 = 0x9A
CMD_CLR_ERROR    = 0x9B
CMD_READ_STATUS2 = 0x9C
CMD_MOTOR_OFF    = 0x80
CMD_MOTOR_ON     = 0x88
CMD_MOTOR_STOP   = 0x81


def _clamp_int16(x: int) -> int:
    """限制到 int16 范围（-32768..32767），随后按补码打包。"""
    return max(-32768, min(32767, int(x)))


def _pack_i16_le(x: int) -> Tuple[int, int]:
    """int16 -> (low_byte, high_byte) 小端。"""
    x = _clamp_int16(x)
    u = x & 0xFFFF
    return (u & 0xFF, (u >> 8) & 0xFF)


def build_torque_broadcast(t1: int, t2: int, t3: int, t4: int = 0) -> can.Message:
    """
    帧ID 0x280:
    data[0..1]=#1 torqueValue (low,high)
    data[2..3]=#2
    data[4..5]=#3
    data[6..7]=#4
    """
    d0, d1 = _pack_i16_le(t1)
    d2, d3 = _pack_i16_le(t2)
    d4, d5 = _pack_i16_le(t3)
    d6, d7 = _pack_i16_le(t4)
    data = [d0, d1, d2, d3, d4, d5, d6, d7]
    return can.Message(arbitration_id=TORQUE_CMD_ID, is_extended_id=False, data=data)


def build_mixed_cmd(cmd1: int, cmd2: int, cmd3: int, cmd4: int = 0x00) -> can.Message:
    """
    帧ID 0x288:
    data[0]=#1 motorCmd, data[1]=0
    data[2]=#2 motorCmd, data[3]=0
    data[4]=#3 motorCmd, data[5]=0
    data[6]=#4 motorCmd, data[7]=0
    """
    data = [cmd1 & 0xFF, 0x00, cmd2 & 0xFF, 0x00, cmd3 & 0xFF, 0x00, cmd4 & 0xFF, 0x00]
    return can.Message(arbitration_id=MIXED_CMD_ID, is_extended_id=False, data=data)


def send_many(bus: can.BusABC, msgs: Iterable[can.Message], gap_s: float = 0.005) -> None:
    """连续发几帧（给驱动一点处理间隔）。"""
    for m in msgs:
        bus.send(m)
        print(m)
        time.sleep(gap_s)


def main():
    # 1) 选择 CAN 接口
    # - Linux SocketCAN: channel="can0", bustype="socketcan"
    # - 如果你用的是 USB-CAN 且已有 python-can 对应驱动，也可改 bustype/channel
    bus = can.Bus(interface="canalystii", channel=0, bitrate=1000000)

    # 2) 可选：先广播“电机开启”
    send_many(bus, [build_mixed_cmd(CMD_MOTOR_ON, CMD_MOTOR_ON, CMD_MOTOR_ON, CMD_MOTOR_ON)], gap_s=0.02)

    # 3) 力矩模式：控制 3 个电机一起转（示例：#1=+T, #2=-T, #3=+T，第4路=0）
    #    注意 torqueValue 的具体量程与含义：文档写 MF/MG 为 -2000..2000（电流力矩），MS 为 -850..850（开环电压）
    #    发送频率：文档提示 500kbps 下 4 电机广播最大约 600Hz；1Mbps 下约 1.2kHz。这里示例用 200Hz 更稳妥。
    freq_hz = 500.0
    dt = 1.0 / freq_hz

    T = 200  # 示例力矩指令（根据你的电机系列调整范围）

    count = 0

    try:
        while True:
            # now = time.time()
            # if now - t0 > duration_s:
            #     break
    
            msg = build_torque_broadcast(
                t1=50,
                t2=50,
                t3=40,
                t4=50
            )
            bus.send(msg)

            count += 1
            if count % 100 == 0:
                print(msg)

            time.sleep(dt)

    except KeyboardInterrupt:
        pass
    finally:
        # 4) 停止（建议退出前先 stop，再按需 off）
        msg = build_torque_broadcast(
            t1=0,
            t2=0,
            t3=0,
            t4=0
        )
        bus.send(msg)
        send_many(bus, [build_mixed_cmd(CMD_MOTOR_STOP, CMD_MOTOR_STOP, CMD_MOTOR_STOP, CMD_MOTOR_STOP)], gap_s=0.02)
        send_many(bus, [build_mixed_cmd(CMD_MOTOR_OFF, CMD_MOTOR_OFF, CMD_MOTOR_OFF, CMD_MOTOR_OFF)], gap_s=0.02)
        bus.shutdown()


if __name__ == "__main__":
    main()