"""
CTAG2F90D（Servo）控制器SDK (基于Modbus RTU的RS-485通信)
---------------------------------------------
一个轻量级的Python SDK，用于通过minimalmodbus库控制电机，
具有线程安全、清晰的寄存器访问方式和实时数据监控功能。

依赖安装:
    pip install minimalmodbus pyserial

作者: 知行机器人
"""

import time
import threading
import minimalmodbus
import serial

# -----------------------------
# 寄存器映射 (保持寄存器，功能码0x03/0x06)
# -----------------------------
# 写入寄存器 (目标值/控制)
REG_TARGET_POS_HIGH       = 0x0102  # 目标位置（高16位）
REG_TARGET_POS_LOW        = 0x0103  # 目标位置（低16位）
REG_TARGET_SPEED          = 0x0104  # 目标速度
REG_TARGET_FORCE          = 0x0105  # 目标力/力矩
REG_TARGET_ACCELERATION   = 0x0106  # 目标加速度
REG_TARGET_DECELERATION   = 0x0107  # 目标减速度
REG_MOTION_TRIGGER        = 0x0108  # 运动触发（0: 空闲, 1: 触发）

# 读取寄存器 (反馈/状态)
REG_REAL_POS_HIGH         = 0x0418  # 实时位置（高16位）
REG_REAL_POS_LOW          = 0x0419  # 实时位置（低16位）
REG_REAL_SPEED            = 0x041A  # 实时速度反馈
REG_REAL_CURRENT          = 0x041B  # 实时电流反馈


class MotorController:
    def __init__(self, port: str, slave_id: int = 1, baudrate: int = 115200, timeout: float = 1.0):
        """初始化与电机控制器的Modbus RTU通信

        参数:
            port: 串口端口（例如，Windows上为'COM3'，Linux上为'/dev/ttyUSB0'）
            slave_id: Modbus从机地址（默认: 1）
            baudrate: 串口波特率（默认: 115200）
            timeout: 读写超时时间（秒，默认: 1.0）
        """
        self.instrument = minimalmodbus.Instrument(port, slave_id)
        self.instrument.serial.baudrate = baudrate
        self.instrument.serial.bytesize = 8
        self.instrument.serial.parity = serial.PARITY_NONE
        self.instrument.serial.stopbits = 1
        self.instrument.serial.timeout = timeout
        self.instrument.mode = minimalmodbus.MODE_RTU
        self._lock = threading.Lock()  # 线程锁，确保串口操作线程安全
        self._monitor_running = False  # 监控线程运行标志
        self._monitor_thread = None    # 监控线程对象

    # -----------------------------
    # 低级寄存器操作工具方法
    # -----------------------------
    def _write_register(self, addr: int, value: int) -> None:
        """写入单个16位保持寄存器（Modbus功能码0x06）

        参数:
            addr: 寄存器地址
            value: 要写入的值
        """
        with self._lock:
            self.instrument.write_register(addr, value, functioncode=6)

    def _read_register(self, addr: int) -> int:
        """读取单个16位保持寄存器（Modbus功能码0x03）

        参数:
            addr: 寄存器地址

        返回:
            寄存器的值
        """
        with self._lock:
            return self.instrument.read_register(addr, functioncode=3)

    def _write_registers(self, addr: int, values: list) -> None:
        """写入多个16位保持寄存器（Modbus功能码0x10）

        参数:
            addr: 起始寄存器地址
            values: 要写入的值列表
        """
        with self._lock:
            self.instrument.write_registers(addr, values)

    def _read_registers(self, addr: int, count: int) -> list:
        """读取多个16位保持寄存器（Modbus功能码0x03）

        参数:
            addr: 起始寄存器地址
            count: 要读取的寄存器数量

        返回:
            寄存器值的列表
        """
        with self._lock:
            return self.instrument.read_registers(addr, count, functioncode=3)

    # -----------------------------
    # 目标值/控制写入操作
    # -----------------------------
    def set_target_position(self, position: int) -> None:
        """设置目标位置（组合高低16位寄存器）

        参数:
            position: 目标位置值
        """
        hi = (position >> 16) & 0xFFFF  # 提取高16位
        lo = position & 0xFFFF          # 提取低16位
        self._write_registers(REG_TARGET_POS_HIGH, [hi, lo])

    def set_target_speed(self, speed: int) -> None:
        """设置目标速度

        参数:
            speed: 目标速度值
        """
        self._write_register(REG_TARGET_SPEED, speed)

    def set_target_force(self, force: int) -> None:
        """设置目标力/力矩

        参数:
            force: 目标力/力矩值
        """
        self._write_register(REG_TARGET_FORCE, force)

    def set_target_acceleration(self, acceleration: int) -> None:
        """设置目标加速度

        参数:
            acceleration: 目标加速度值
        """
        self._write_register(REG_TARGET_ACCELERATION, acceleration)

    def set_target_deceleration(self, deceleration: int) -> None:
        """设置目标减速度

        参数:
            deceleration: 目标减速度值
        """
        self._write_register(REG_TARGET_DECELERATION, deceleration)

    def trigger_motion(self) -> None:
        """使用已配置的目标参数触发运动"""
        self._write_register(REG_MOTION_TRIGGER, 1)

    def temp_move(self, position_mm: int, speed_pct: int, force_pct: int,
                  accel: int, decel: int, trigger: bool = True) -> None:
        """临时运动控制方法，一次性设置所有运动参数并可选触发运动

        参数:
            position_mm: 目标位置(毫米)
            speed_pct: 速度百分比
            force_pct: 力/力矩百分比
            accel: 加速度值
            decel: 减速度值
            trigger: 是否立即触发运动，默认为True
        """
        self.set_target_position(position_mm)
        self.set_target_speed(speed_pct)
        self.set_target_force(force_pct)
        self.set_target_acceleration(accel)
        self.set_target_deceleration(decel)

        if trigger:
            self.trigger_motion()
            print(f"已触发运动到位置 {position_mm}，速度 {speed_pct}%，力 {force_pct}%")

    # -----------------------------
    # 反馈/状态读取操作
    # -----------------------------
    def read_real_position(self) -> int:
        """读取实时位置（组合高低16位寄存器）

        返回:
            组合后的32位实时位置（处理有符号值）
        """
        regs = self._read_registers(REG_REAL_POS_HIGH, 2)
        hi, lo = regs[0], regs[1]
        combined = (hi << 16) | lo
        # 处理32位有符号整数
        if combined & 0x80000000:  # 最高位为1表示负数
            combined = combined - 0x100000000
        return combined

    def read_real_speed(self) -> int:
        """读取实时速度反馈

        返回:
            实时速度值
        """
        return self._read_register(REG_REAL_SPEED)

    def read_real_current(self) -> int:
        """读取实时电流反馈

        返回:
            实时电流值
        """
        return self._read_register(REG_REAL_CURRENT)

    # -----------------------------
    # 实时监控线程
    # -----------------------------
    def _monitor_loop(self, interval: float = 0.5) -> None:
        """实时数据监控的后台循环"""
        while self._monitor_running:
            try:
                pos = self.read_real_position()
                speed = self.read_real_speed()
                current = self.read_real_current()
                print(f"[实时数据] 位置: {pos:6d}, 速度: {speed:3d}, 电流: {current:3d}")
            except Exception as e:
                print(f"监控错误: {e}")
            time.sleep(interval)

    def start_monitoring(self, interval: float = 0.01) -> None:
        """启动实时数据监控线程

        参数:
            interval: 监控间隔（秒，默认: 0.01）
        """
        if not self._monitor_running:
            self._monitor_running = True
            self._monitor_thread = threading.Thread(
                target=self._monitor_loop,
                args=(interval,)
            )
            self._monitor_thread.daemon = True  # 守护线程，主程序退出时自动结束
            self._monitor_thread.start()
            print("实时监控已启动。")

    def stop_monitoring(self) -> None:
        """停止实时数据监控线程"""
        self._monitor_running = False
        if self._monitor_thread:
            self._monitor_thread.join(timeout=1.0)  # 等待线程结束
        print("实时监控已停止。")


if __name__ == "__main__":
    # 示例用法（根据实际设备调整端口/从机ID）
    PORT = "COM5"
    SLAVE_ID = 1
    sdk = MotorController(PORT, SLAVE_ID, baudrate=115200, timeout=1.0)

    try:
        # 启动实时监控
        sdk.start_monitoring(interval=0.5)
        
        # 循环控制：5次往复运动
        # 假设 9000 为闭合位置，0 为张开位置
        # 最后一次循环结束后，确保处于张开位置 (0)
        for i in range(5):
            print(f"\n--- 第 {i+1}/5 次循环 ---")
            
            # 阶段1: 运动到位置 9000 (闭合?)
            sdk.temp_move(position_mm=9000, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
            time.sleep(5)  # 等待运动完成
            
            # 阶段2: 运动到位置 0 (张开?)
            sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
            time.sleep(5)  # 等待运动完成
            
        print("\n循环结束，确认最终状态为张开 (位置 0)...")
        # 再次发送指令确保最终状态为 0 (虽然循环最后一步已经是 0，但这保证了"最后要张开"的要求)
        sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
        time.sleep(1)

    except KeyboardInterrupt:
        print("\n程序被用户中断。")
    except Exception as e:
        print(f"执行过程中出错: {e}")
    finally:
        sdk.stop_monitoring()
        print("程序已完成。")
