#!/usr/bin/env python
# 不同模式can发送指令集成
# -*- coding: utf-8 -*-
'''
# @Time    : 2025/7/21
# @Author  : hx
# @File    : motor_control_can_all.py
'''

import can
import time
import argparse


# 细分值映射表
MICROSTEP_MAP = {
    2: 0x01,
    4: 0x02,
    8: 0x04,
    16: 0x10,
    32: 0x20
}

# ========================================
# CAN 发送函数（使用 socketcan）
# ========================================
def send_can_frame(data, can_id=0x01, channel='can0', bustype='socketcan'):
    """
    发送CAN帧
    :param data: 7字节的列表
    :param can_id: CAN ID
    :param channel: CAN 通道
    :param bustype: 总线类型
    """
    try:
        bus = can.interface.Bus(channel=channel, bustype=bustype)
        msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False)
        bus.send(msg)
        print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]")
        bus.shutdown()
    except Exception as e:
        print(f"[发送失败] {e}")


# ========================================
# 模式1：速度控制模式 (0x01)
# ========================================
def generate_speed_command(direction, microstep, speed):
    """
    生成速度控制指令
    :param direction: 方向 (0:逆时针, 1:顺时针)
    :param microstep: 细分值 (2, 4, 8, 16, 32)
    :param speed: 速度值 (单位: rad/s)
    """
    direction_byte = 0x01 if direction == 1 else 0x00
    microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)

    raw_speed = int(speed * 10)
    speed_high = (raw_speed >> 8) & 0xFF
    speed_low = raw_speed & 0xFF
    return [0x01, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low]


# ========================================
# 模式2：位置控制模式 (0x02)
# ========================================
def generate_position_command(direction, microstep, angle):
    """
    生成位置控制指令
    :param direction: 方向 (0:逆时针, 1:顺时针)
    :param microstep: 细分值 (2, 4, 8, 16, 32)
    :param angle: 角度值 (单位: 度)
    """
    direction_byte = 0x01 if direction == 1 else 0x00
    microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)

    raw_angle = int(angle * 10)
    pos_high = (raw_angle >> 8) & 0xFF
    pos_low = raw_angle & 0xFF

    return [0x02, direction_byte, microstep_byte, pos_high, pos_low, 0x00, 0x64]


# ========================================
# 模式3：力矩控制模式 (0x03)
# ========================================
def generate_torque_command(direction, microstep, current):
    """
    生成力矩控制指令
    :param direction: 方向 (0:逆时针, 1:顺时针)
    :param microstep: 细分值 (2, 4, 8, 16, 32)
    :param current: 电流值 (单位: mA)
    """
    direction_byte = 0x01 if direction == 1 else 0x00
    microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)

    raw_current = int(current)
    current_high = (raw_current >> 8) & 0xFF
    current_low = raw_current & 0xFF

    return [0x03, direction_byte, microstep_byte, current_high, current_low, 0x00, 0x64]


# ========================================
# 模式4：单圈绝对角度控制模式 (0x04)
# ========================================
def generate_absolute_angle_command(direction, microstep, angle, speed):
    """
    生成单圈绝对角度控制指令
    :param direction: 方向 (0:逆时针, 1:顺时针)
    :param microstep: 细分值 (2, 4, 8, 16, 32)
    :param angle: 目标角度 (单位: 度)
    :param speed: 速度值 (单位: rad/s)
    """
    direction_byte = 0x01 if direction == 1 else 0x00
    microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)

    raw_angle = int(angle * 10)
    pos_high = (raw_angle >> 8) & 0xFF
    pos_low = raw_angle & 0xFF

    raw_speed = int(speed * 10)
    speed_high = (raw_speed >> 8) & 0xFF
    speed_low = raw_speed & 0xFF


    return [0x04, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low]


# ========================================
# 主函数（命令行调用）
# ========================================
def main():
    parser = argparse.ArgumentParser(description="CAN电机控制程序，支持4种模式")
    parser.add_argument("--mode", type=int, choices=[1, 2, 3, 4],
                        help="控制模式: 1=速度, 2=位置, 3=力矩, 4=绝对角度")
    parser.add_argument("--direction", type=int, choices=[0, 1], default=1,
                        help="方向: 0=逆时针, 1=顺时针")
    parser.add_argument("--microstep", type=int, choices=[2, 4, 8, 16, 32], default=32,
                        help="细分值: 2, 4, 8, 16, 32")
    parser.add_argument("--value", type=float, required=True,
                        help="控制值（速度: rad/s, 位置/角度: 度, 力矩: mA）")
    parser.add_argument("--speed_rad_per_sec", type=float,
                        help="速度值（仅用于绝对角度模式）")
    args = parser.parse_args()

    try:
        if args.mode == 1:
            cmd = generate_speed_command(args.direction, args.microstep, args.value)
        elif args.mode == 2:
            cmd = generate_position_command(args.direction, args.microstep, args.value)
        elif args.mode == 3:
            cmd = generate_torque_command(args.direction, args.microstep, args.value)
        elif args.mode == 4:
            if args.speed_rad_per_sec is None:
                raise ValueError("绝对角度模式需要提供速度参数 (--speed_rad_per_sec)")
            cmd = generate_absolute_angle_command(args.direction, args.microstep, args.value, args.speed_rad_per_sec)

        print(f"生成CAN指令: [{', '.join(f'0x{x:02X}' for x in cmd)}]")
        send_can_frame(cmd)

    except Exception as e:
        print(f"错误: {e}")


# ========================================
# 程序入口
# ========================================
if __name__ == "__main__":
    main()
    #python motor_control_can_all.py --mode 1 --direction 1 --microstep 32 --value 10
    #python motor_control_can_all.py --mode 2 --direction 1 --microstep 32 --value 1000
    #python motor_control_can_all.py --mode 3 --direction 1 --microstep 32 --value 1000