#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time    : 2025/7/15 17:52
# @Author  : hx
# @File    : motor.py
'''

import time

start_speed = 8
stop_speed = 0

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

def generate_speed_command(direction=1, microstep=32, speed=10):
    """
    生成速度控制模式的 CAN 数据帧（7字节）
    :param direction: 0=逆时针，1=顺时针
    :param microstep: 细分值（2, 4, 8, 16, 32）
    :param speed: 速度值（Rad/s）
    :return: 7字节 CAN 数据帧
    """
    control_mode = 0x01  # 速度控制模式
    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 [control_mode, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low]


def generate_position_command(angle, speed=10, direction=1, microstep=32):
    """
    生成位置控制模式的 CAN 数据帧（7字节）
    :param angle: 目标角度（度）
    :param speed: 速度值（Rad/s）
    :param direction: 0=逆时针，1=顺时针
    :param microstep: 细分值（2, 4, 8, 16, 32）
    :return: 7字节 CAN 数据帧
    """
    control_mode = 0x02  # 位置控制模式
    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 [control_mode, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low]


def send_can_frame(data, can_id=0x01):
    """
    模拟发送CAN帧（只打印，不实际发送）
    """
    print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]")


def listen_and_respond():
    """
    模拟监听并响应信号的逻辑
    """
    try:
        print("[监听中] 等待信号...")

        # 模拟收到信号后执行动作
        time.sleep(2)

        # 示例：位置控制模式 - 顺时针转动 1000 度，速度 10Rad/s，32细分
        pos_data = generate_position_command(angle=1000, speed=10, direction=1, microstep=32)
        send_can_frame(pos_data, can_id=0x01)

        time.sleep(2)

        # 示例：速度控制模式 - 顺时针，速度 8Rad/s，32细分
        speed_data = generate_speed_command(direction=1, microstep=32, speed=8)
        send_can_frame(speed_data, can_id=0x01)

        print("[已完成一次响应]")

    except Exception as e:
        print(f"监听时发生错误: {e}")


if __name__ == "__main__":
    try:
        # 示例：执行一次电机控制流程
        listen_and_respond()

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