# qiege_motor.py
# 切割电机控制模块（支持 CAN 和 USB 两种通信方式）
# 使用 control(enable) 统一接口

import time
import serial

# ------------------------ 配置区 ------------------------

# <<< 切换通信模式 >>>
MODE = 'usb'  # 'can' 或 'usb'

# CAN 模式配置
CAN_SERIAL_PORT = '/dev/ttyACM0'  # CAN 转串口设备
CAN_BAUDRATE = 921600
CAN_MOTOR_ID = 0x05  # 轮趣电机对应的 CAN ID

# USB 串口模式配置（轮趣 HEX 指令）
USB_SERIAL_PORT = '/dev/ttyACM0'  # 或 '/dev/ttyUSB0'
USB_BAUDRATE = 115200

# 目标转速（rad/s）
CUTTING_SPEED_RAD_PER_SEC = 8.0  # 对应 HEX 指令中的 speed=8 → 80 (0x50)

# 是否启用调试输出
DEBUG = True

# ------------------------ 全局变量 ------------------------
usb_serial = None
can_serial = None


# ------------------------ USB 模式：HEX 指令生成与发送 ------------------------
def generate_usb_command(speed):
    """
    生成轮趣电机 USB 串口 HEX 指令
    格式: 7B 01 01 00 20 00 00 00 <speed_byte> <checksum> 7D
    speed: float (rad/s), 映射为 0~255
    """
    # 将 rad/s 映射为速度字节（示例：8 rad/s → 80）
    # 你可以根据实际响应调整映射关系
    speed_val = int(speed * 10)  # 示例：8 → 80
    speed_byte = max(0, min(255, speed_val))

    # 构造前9字节
    cmd = [
        0x7B, 0x01, 0x02,     # 帧头、地址、模式
        0x01, 0x20,           # 方向、细分
        0x0E, 0x10,           # 位置高、低（持续旋转）
        0x00, 0x64,  # 速度值
    ]

    # 计算校验和（前9字节异或）
    checksum = 0
    for b in cmd:
        checksum ^= b
    cmd.append(checksum)
    cmd.append(0x7D)  # 帧尾

    return bytes(cmd)


def send_usb_command(port, baudrate, command):
    """发送 USB 串口指令"""
    global usb_serial
    try:
        if usb_serial is None or not usb_serial.is_open:
            usb_serial = serial.Serial(port, baudrate, timeout=0.5)
            time.sleep(0.5)  # 等待串口稳定

        usb_serial.write(command)
        if DEBUG:
            print(f"[qiege_motor] USB 发送: {command.hex().upper()}")
        return True
    except Exception as e:
        if DEBUG:
            print(f"[qiege_motor] USB 发送失败: {e}")
        usb_serial = None
        return False


# ------------------------ CAN 模式：CAN 指令发送 ------------------------
def send_can_command(data, can_id=CAN_MOTOR_ID):
    """通过 CAN 发送指令（假设使用串口转 CAN 模块）"""
    global can_serial
    try:
        if can_serial is None or not can_serial.is_open:
            can_serial = serial.Serial(CAN_SERIAL_PORT, CAN_BAUDRATE, timeout=0.5)
            time.sleep(0.5)

        # 示例：简单封装 CAN 帧（ID + DLC + Data）
        # 格式根据你使用的 CAN 模块调整（如 CANable、TJA1050 等）
        can_frame = [
            (can_id >> 24) & 0xFF,
            (can_id >> 16) & 0xFF,
            (can_id >> 8) & 0xFF,
            can_id & 0xFF,
            len(data)  # DLC
        ] + list(data)
        can_serial.write(bytes(can_frame))

        if DEBUG:
            print(f"[qiege_motor] CAN 发送 → ID:{can_id:05X}, Data:{[f'{b:02X}' for b in data]}")
        return True
    except Exception as e:
        if DEBUG:
            print(f"[qiege_motor] CAN 发送失败: {e}")
        can_serial = None
        return False


def generate_can_command(speed_rpm=300, direction=1, microstep=32):
    """生成轮趣 CAN 速度指令（示例）"""
    speed_01rpm = int(abs(speed_rpm) * 10)
    high_byte = (speed_01rpm >> 8) & 0xFF
    low_byte = speed_01rpm & 0xFF
    direction_bit = 1 if direction else 0

    data = [
        0x88,  # 控制字：速度模式
        ((direction_bit & 0x01) << 7) | (microstep & 0x7F),
        0x00, 0x00,
        high_byte, low_byte,
        0x00, 0x00
    ]
    return data


# ------------------------ 统一控制接口 ------------------------
def start_motor():
    """启动切割电机"""
    if MODE == 'usb':
        cmd = generate_usb_command(CUTTING_SPEED_RAD_PER_SEC)
        return send_usb_command(USB_SERIAL_PORT, USB_BAUDRATE, cmd)
    elif MODE == 'can':
        # 示例：8 rad/s ≈ 76.4 RPM
        rpm = CUTTING_SPEED_RAD_PER_SEC * 60 / (2 * 3.1416)
        cmd = generate_can_command(speed_rpm=rpm, direction=1)
        return send_can_command(cmd, can_id=CAN_MOTOR_ID)
    else:
        print(f"[qiege_motor] 错误：不支持的模式 '{MODE}'")
        return False


def stop_motor():
    """停止切割电机"""
    if MODE == 'usb':
        cmd = generate_usb_command(0.0)
        return send_usb_command(USB_SERIAL_PORT, USB_BAUDRATE, cmd)
    elif MODE == 'can':
        cmd = generate_can_command(speed_rpm=0, direction=1)
        return send_can_command(cmd, can_id=CAN_MOTOR_ID)
    else:
        return False


def control(enable: bool):
    """
    统一控制接口
    :param enable: True 启动, False 停止
    :return: bool 成功与否
    """
    try:
        if enable:
            return start_motor()
        else:
            return stop_motor()
    except Exception as e:
        if DEBUG:
            print(f"[qiege_motor] 控制异常: {e}")
        return False


# ------------------------ 测试 ------------------------
if __name__ == "__main__":
    print(f"【qiege_motor】测试开始 (模式: {MODE})")
    print("启动 → 1秒后停止")

    control(True)
    time.sleep(1)
    control(False)

    print("测试结束。")