using Common.Attributes;
using DeviceCommand.Base;
using Model.Models;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Threading;
using System.ComponentModel;

namespace DeviceCommand.Devices
{
    [ADPCommand]
    public class GantryControlTcp : ModbusTcp
    {
        private readonly byte _slaveAddress=1;

        // 向后兼容：TcpDevice 即自身（继承自 ModbusTcp）
        public ModbusTcp TcpDevice => this;

        // =================================================================
        // ------------------ 软件位置跟踪（绝对位置寄存器会被清零）----------
        // =================================================================
        /// <summary>当前位置相对于机械零点的位置（每次移动后累加偏移）</summary>
        private int _originX, _originY, _originZ;
        /// <summary>是否已完成回零初始化</summary>
        private bool _isHomed;

        public GantryControlTcp(TcpConfig config) : base(config)
        {

        }

        // =================================================================
        // ------------------ Coil / Bit 状态与控制 (位元件) ------------------
        // =================================================================
        private const ushort ADDR_M_ESTOP = 40;          // 0x5010: 紧停
        private const ushort ADDR_M_X_STOP = 41;            // 0x29: X轴停止
        private const ushort ADDR_M_Y_STOP = 42;            // 0x2A: Y轴停止
        private const ushort ADDR_M_Z_STOP = 43;            // 0x2B: Z轴停止

        private const ushort ADDR_M_X_START = 30;           // 0x1E: X轴指定位置启动
        private const ushort ADDR_M_Y_START = 31;           // 0x1F: Y轴指定位置启动
        private const ushort ADDR_M_Z_START = 35;           // 0x23: Z轴指定位置启动

        private const ushort ADDR_M_HOME_TRIGGER = 100;     // 0x64: 回原点触发

        // =================================================================
        // ------------------ 轴使能控制 (HM 寄存器) ------------------
        // =================================================================
        private const ushort ADDR_HM_AXIS1_ENABLE = 49408;  // 0xC100: 轴1使能 (X轴)
        private const ushort ADDR_HM_AXIS2_ENABLE = 49409;  // 0xC101: 轴2使能 (Y轴)
        private const ushort ADDR_HM_AXIS3_ENABLE = 49410;  // 0xC102: 轴3使能 (Z轴)
        private const ushort ADDR_HM_AXIS4_ENABLE = 49411;  // 0xC103: 轴4使能
        private const ushort ADDR_HM_AXIS5_ENABLE = 49412;  // 0xC104: 轴5使能
        private const ushort ADDR_HM_AXIS6_ENABLE = 49413;  // 0xC105: 轴6使能

        // =================================================================
        // ------------------ 速度与位置参数 (HD 32位数据寄存器) --------------
        // =================================================================
        private const ushort ADDR_HD_HOME_POS_SPEED = 41288;   // 0xA148: 原点定位速度 (32位)

        // 轴原点位置
        private const ushort ADDR_HD_X_HOME_POS = 41198;      // 0xA0EE: X轴原点位 (32位)
        private const ushort ADDR_HD_Z_HOME_POS = 41208;      // 0xA0F8: Z轴原点位 (32位)
        private const ushort ADDR_HD_Y_HOME_POS = 41218;      // 0xA102: Y轴原点位 (32位)

        // 轴运行目标位置
        private const ushort ADDR_HD_Y_TARGET = 41228;        // 0xA10C: Y轴指定位置 (32位)
        private const ushort ADDR_HD_Z_TARGET = 41238;        // 0xA116: Z轴指定位置 (32位)
        private const ushort ADDR_HD_X_TARGET = 41248;        // 0xA120: X轴指定位置 (32位)

        // 轴运行速度
        private const ushort ADDR_HD_X_SPEED = 41318;         // 0xA166: X轴速度 (32位)
        private const ushort ADDR_HD_Z_SPEED = 41328;         // 0xA170: Z轴速度 (32位)
        private const ushort ADDR_HD_Y_SPEED = 41338;         // 0xA17A: Y轴速度 (32位)

        // =================================================================
        // ------------------ 轴绝对位置（只读，相对原点）------------------
        // =================================================================
        private const ushort ADDR_ABS_AXIS1_POS = 47232;      // 轴1绝对位置 (32位)
        private const ushort ADDR_ABS_AXIS2_POS = 47236;      // 轴2绝对位置 (32位)
        private const ushort ADDR_ABS_AXIS3_POS = 47240;      // 轴3绝对位置 (32位)
        private const ushort ADDR_ABS_AXIS4_POS = 47244;      // 轴4绝对位置 (32位)
        private const ushort ADDR_ABS_AXIS5_POS = 47248;      // 轴5绝对位置 (32位)
        private const ushort ADDR_ABS_AXIS6_POS = 47252;      // 轴6绝对位置 (32位)

        // =================================================================
        // ------------------ 核心控制逻辑与参数读写 ------------------------
        // =================================================================

        /// <summary>
        /// 轴使能控制
        /// </summary>
        /// <param name="轴编号">轴编号 (1 - 6)</param>
        /// <param name="使能">true: 使能轴, false: 禁用轴</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 设置轴使能(int 轴编号, bool 使能, CancellationToken 取消令牌 = default)
        {
            ushort address = 轴编号 switch
            {
                1 => ADDR_HM_AXIS1_ENABLE,
                2 => ADDR_HM_AXIS2_ENABLE,
                3 => ADDR_HM_AXIS3_ENABLE,
                4 => ADDR_HM_AXIS4_ENABLE,
                5 => ADDR_HM_AXIS5_ENABLE,
                6 => ADDR_HM_AXIS6_ENABLE,
                _ => throw new ArgumentException("轴通道错误，仅支持 1 - 6", nameof(轴编号))
            };

            await WriteSingleCoilAsync(_slaveAddress, address, 使能, 取消令牌);
        }

        /// <summary>
        /// 读取指定轴的使能状态
        /// </summary>
        /// <param name="轴编号">1: X轴, 2: Y轴, 3: Z轴</param>
        /// <param name="取消令牌">取消令牌</param>
        /// <returns>true: 已使能, false: 未使能</returns>
        public async Task<bool> 查询轴使能(int 轴编号, CancellationToken 取消令牌 = default)
        {
            ushort address = 轴编号 switch
            {
                1 => ADDR_HM_AXIS1_ENABLE,
                2 => ADDR_HM_AXIS2_ENABLE,
                3 => ADDR_HM_AXIS3_ENABLE,
                _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(轴编号))
            };

            bool[] coils = await ReadCoilsAsync(_slaveAddress, address, 1, 取消令牌);
            return coils != null && coils.Length > 0 && coils[0];
        }

        /// <summary>
        /// 同时设置 X/Y/Z 轴的原点位置（带各自独立的范围检查）
        /// </summary>
        /// <param name="X轴原点">X轴原点位置 (范围: 0 ~ 50000)</param>
        /// <param name="Y轴原点">Y轴原点位置 (范围: 0 ~ 50000)</param>
        /// <param name="Z轴原点">Z轴原点位置 (范围: 0 ~ 20000)</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 设置原点位置(int X轴原点, int Y轴原点, int Z轴原点, CancellationToken 取消令牌 = default)
        {
            if (X轴原点 < 0 || X轴原点 > 50000)
                throw new ArgumentOutOfRangeException(nameof(X轴原点), $"X轴原点设置值 [{X轴原点}] 超出合法范围 (0 - 50000)");

            if (Y轴原点 < 0 || Y轴原点 > 50000)
                throw new ArgumentOutOfRangeException(nameof(Y轴原点), $"Y轴原点设置值 [{Y轴原点}] 超出合法范围 (0 - 50000)");

            if (Z轴原点 < 0 || Z轴原点 > 20000)
                throw new ArgumentOutOfRangeException(nameof(Z轴原点), $"Z轴原点设置值 [{Z轴原点}] 超出合法范围 (0 - 20000)");

            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_HOME_POS, Int32ToUshorts(X轴原点), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_HOME_POS, Int32ToUshorts(Y轴原点), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_HOME_POS, Int32ToUshorts(Z轴原点), 取消令牌);
        }

        /// <summary>
        /// 批量查询当前 X、Y、Z 三轴设定的原点位置值
        /// </summary>
        /// <param name="取消令牌">取消令牌</param>
        /// <returns>返回包含三轴原点位置的元组 (X, Y, Z)</returns>
        public async Task<(int X, int Y, int Z)> 查询原点位置(CancellationToken 取消令牌 = default)
        {
            // 41218(Y) - 41198(X) = 20，加Y轴自身的 2 个寄存器，共批量读取 22 个连续寄存器
            ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, ADDR_HD_X_HOME_POS, 22, 取消令牌);

            if (registers == null || registers.Length < 22)
                throw new InvalidOperationException("从 PLC 读取三轴原点位置失败，返回数据长度不足。");

            int xHomePos = UshortsToInt32(new ushort[] { registers[0], registers[1] });
            int zHomePos = UshortsToInt32(new ushort[] { registers[10], registers[11] });
            int yHomePos = UshortsToInt32(new ushort[] { registers[20], registers[21] });

            return (xHomePos, yHomePos, zHomePos);
        }

        /// <summary>
        /// 设置回原点定位速度（带 0 - 100000 范围检查）
        /// </summary>
        /// <param name="速度">回原点定位速度 (范围: 0 - 100000)</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 设置回零速度(int 速度, CancellationToken 取消令牌 = default)
        {
            if (速度 < 0 || 速度 > 100000)
                throw new ArgumentOutOfRangeException(nameof(速度), $"回原点定位速度值 [{速度}] 超出合法范围 (0 - 100000)");

            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_HOME_POS_SPEED, Int32ToUshorts(速度), 取消令牌);
        }

        /// <summary>
        /// 查询当前设定的回原点定位速度
        /// </summary>
        /// <param name="取消令牌">取消令牌</param>
        /// <returns>当前回原点定位速度值</returns>
        public async Task<int> 查询回零速度(CancellationToken 取消令牌 = default)
        {
            ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, ADDR_HD_HOME_POS_SPEED, 2, 取消令牌);

            if (registers == null || registers.Length < 2)
                throw new InvalidOperationException("从 PLC 读取回原点定位速度失败，返回数据长度不足。");

            return UshortsToInt32(registers);
        }

        /// <summary>
        /// 设置指定轴的运行速度（带 0 - 100000 范围检查）
        /// </summary>
        /// <param name="轴编号">1: X轴, 2: Y轴, 3: Z轴</param>
        /// <param name="速度">运行速度 (范围: 0 - 100000)</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 设置轴速度(int 轴编号, int 速度, CancellationToken 取消令牌 = default)
        {
            if (速度 < 0 || 速度 > 100000)
                throw new ArgumentOutOfRangeException(nameof(速度), $"轴速度设置值 [{速度}] 超出合法范围 (0 - 100000)");

            ushort address = 轴编号 switch
            {
                1 => ADDR_HD_X_SPEED,
                2 => ADDR_HD_Y_SPEED,
                3 => ADDR_HD_Z_SPEED,
                _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(轴编号))
            };

            await WriteMultipleRegistersAsync(_slaveAddress, address, Int32ToUshorts(速度), 取消令牌);
        }

        /// <summary>
        /// 读取指定轴当前设定的运行速度
        /// </summary>
        /// <param name="轴编号">1: X轴, 2: Y轴, 3: Z轴</param>
        /// <param name="取消令牌">取消令牌</param>
        /// <returns>当前设定的运行速度值</returns>
        public async Task<int> 查询轴速度(int 轴编号, CancellationToken 取消令牌 = default)
        {
            ushort address = 轴编号 switch
            {
                1 => ADDR_HD_X_SPEED,
                2 => ADDR_HD_Y_SPEED,
                3 => ADDR_HD_Z_SPEED,
                _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(轴编号))
            };

            ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, address, 2, 取消令牌);
            if (registers == null || registers.Length < 2)
                throw new InvalidOperationException($"从 PLC 读取轴 {轴编号} 速度失败。");

            return UshortsToInt32(registers);
        }

        /// <summary>
        /// 写入指定轴的目标位置（动态安全校验：目标位置不能超过 [轴最大物理极限 - 当前轴原点设置值]）
        /// </summary>
        /// <param name="轴编号">1: X轴, 2: Y轴, 3: Z轴</param>
        /// <param name="目标位置">目标位置</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 设置轴目标位置(int 轴编号, int 目标位置, CancellationToken 取消令牌 = default)
        {
            var (xHome, yHome, zHome) = await 查询原点位置(取消令牌);

            ushort targetAddress;
            int maxLimit;
            int currentHomePos;

            switch (轴编号)
            {
                case 1:
                    targetAddress = ADDR_HD_X_TARGET;
                    maxLimit = 50000;
                    currentHomePos = xHome;
                    break;
                case 2:
                    targetAddress = ADDR_HD_Y_TARGET;
                    maxLimit = 50000;
                    currentHomePos = yHome;
                    break;
                case 3:
                    targetAddress = ADDR_HD_Z_TARGET;
                    maxLimit = 20000;
                    currentHomePos = zHome;
                    break;
                default:
                    throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(轴编号));
            }

            int allowedMaxLimit = maxLimit - currentHomePos;

            if (目标位置 < 0 || 目标位置 > allowedMaxLimit)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(目标位置),
                    $"轴 {轴编号} 目标位置 [{目标位置}] 不合法！当前原点为 [{currentHomePos}]，允许的有效写入范围为: 0 - {allowedMaxLimit}"
                );
            }

            await WriteMultipleRegistersAsync(_slaveAddress, targetAddress, Int32ToUshorts(目标位置), 取消令牌);
        }

        /// <summary>
        /// 读取指定轴当前设定的目标指定位置
        /// </summary>
        /// <param name="轴编号">1: X轴, 2: Y轴, 3: Z轴</param>
        /// <param name="取消令牌">取消令牌</param>
        /// <returns>当前设定的目标位置值</returns>
        public async Task<int> 查询轴目标位置(int 轴编号, CancellationToken 取消令牌 = default)
        {
            ushort address = 轴编号 switch
            {
                1 => ADDR_HD_X_TARGET,
                2 => ADDR_HD_Y_TARGET,
                3 => ADDR_HD_Z_TARGET,
                _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(轴编号))
            };

            ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, address, 2, 取消令牌);

            if (registers == null || registers.Length < 2)
                throw new InvalidOperationException($"从 PLC 读取轴 {轴编号} 目标位置失败。");

            return UshortsToInt32(registers);
        }

        /// <summary>
        /// 设置系统紧停锁定状态 (true: 锁定激活, false: 解除锁定)
        /// </summary>
        /// <param name="状态">true: 锁定激活, false: 解除锁定</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 设置急停状态(bool 状态, CancellationToken 取消令牌 = default)
        {
            await WriteSingleCoilAsync(_slaveAddress, ADDR_M_ESTOP, 状态, 取消令牌);
        }

        /// <summary>
        /// 设置指定轴的停止控制状态 (true: 持续输出停止信号, false: 释放停止信号)
        /// </summary>
        /// <param name="轴编号">1: X轴, 2: Y轴, 3: Z轴</param>
        /// <param name="状态">true: 持续输出停止信号, false: 释放停止信号</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 设置轴停止状态(int 轴编号, bool 状态, CancellationToken 取消令牌 = default)
        {
            ushort address = 轴编号 switch
            {
                1 => ADDR_M_X_STOP,
                2 => ADDR_M_Y_STOP,
                3 => ADDR_M_Z_STOP,
                _ => throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(轴编号))
            };

            await WriteSingleCoilAsync(_slaveAddress, address, 状态, 取消令牌);
        }

        /// <summary>
        /// 全局回零（安全校验版：先检查回原点速度是否为 0，为0则拒绝触发）。
        /// 回零完成后读取绝对位置寄存器保存为初始值，重置累计偏移。
        /// </summary>
        /// <param name="回零点等待时间">回零点等待时间</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 回零(int 回零点等待时间, CancellationToken 取消令牌 = default)
        {
            int currentSpeed = await 查询回零速度(取消令牌);

            if (currentSpeed == 0)
                throw new InvalidOperationException("无法触发回原点！当前回原点定位速度为 0，请先设置合法的回零速度。");

            // 1. 触发回零
            await TriggerCoilAsync(ADDR_M_HOME_TRIGGER, 取消令牌);

            // 2. 等待到达机械零点（绝对位置 = 0）
            await 等待到达零点(回零点等待时间, 取消令牌);
            await Task.Delay(500, 取消令牌);

            // 3. 清零绝对位置寄存器
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS2_POS, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS3_POS, Int32ToUshorts(0), 取消令牌);

            // 4. 设置当前位置为机械零点 (0, 0, 0)
            // 设置目标位置
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_TARGET, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_TARGET, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_TARGET, Int32ToUshorts(0), 取消令牌);
        
            // 5. 读取配置的待定位置（原点位置）
            var (xHome, yHome, zHome) = await 查询原点位置(取消令牌);

            // 6. 移动到待定位置
            if (xHome != 0 || yHome != 0 || zHome != 0)
            {

                _originX = xHome;
                _originY = yHome;
                _originZ = zHome;

                // 启动三轴联动
                await 启动全部轴(取消令牌);

                // 等待到达待定位置
                await 等待到达待定位置(xHome, yHome, zHome, 回零点等待时间, 取消令牌);

                // 7. 到达后清零绝对位置寄存器
                await Task.Delay(500, 取消令牌);
                await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, Int32ToUshorts(0), 取消令牌);
                await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS2_POS, Int32ToUshorts(0), 取消令牌);
                await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS3_POS, Int32ToUshorts(0), 取消令牌);

                // 清零目标寄存器
                await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_TARGET, Int32ToUshorts(0), 取消令牌);
                await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_TARGET, Int32ToUshorts(0), 取消令牌);
                await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_TARGET, Int32ToUshorts(0), 取消令牌);
            }

            // 8. 更新当前位置为待定位置
            _originX = xHome;
            _originY = yHome;
            _originZ = zHome;
            _isHomed = true;
        }

        /// <summary>
        /// 单轴指定位置启动（含速度判定、使能自动补齐、动作目标位置独立范围校验）
        /// </summary>
        /// <param name="轴编号">1: X轴, 2: Y轴, 3: Z轴</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 启动单轴(int 轴编号, CancellationToken 取消令牌 = default)
        {
            ushort startAddress;
            int maxLimit;
            int currentHomePos;

            // 1. 获取基础参数与校验边界
            var (xHome, yHome, zHome) = await 查询原点位置(取消令牌);
            switch (轴编号)
            {
                case 1:
                    startAddress = ADDR_M_X_START;
                    maxLimit = 50000;
                    currentHomePos = xHome;
                    break;
                case 2:
                    startAddress = ADDR_M_Y_START;
                    maxLimit = 50000;
                    currentHomePos = yHome;
                    break;
                case 3:
                    startAddress = ADDR_M_Z_START;
                    maxLimit = 20000;
                    currentHomePos = zHome;
                    break;
                default:
                    throw new ArgumentException("轴通道错误，仅支持 1(X), 2(Y), 3(Z)", nameof(轴编号));
            }

            // 2. 检查动作轴的速度
            int currentSpeed = await 查询轴速度(轴编号, 取消令牌);
            if (currentSpeed == 0)
                throw new InvalidOperationException($"轴 {轴编号} 无法启动！当前设定速度为 0。");

            // 3. 批量检查 1~6 号轴使能状态，若任意轴未使能则全部补写 true
            bool[] allEnableStates = await ReadCoilsAsync(_slaveAddress, ADDR_HM_AXIS1_ENABLE, 6, 取消令牌);
            if (allEnableStates == null || allEnableStates.Length < 6)
                throw new InvalidOperationException("从 PLC 读取 1~6 号轴使能状态失败。");

            bool hasModifiedEnable = false;
            ushort[] enableAddresses = { ADDR_HM_AXIS1_ENABLE, ADDR_HM_AXIS2_ENABLE, ADDR_HM_AXIS3_ENABLE, ADDR_HM_AXIS4_ENABLE, ADDR_HM_AXIS5_ENABLE, ADDR_HM_AXIS6_ENABLE };

            for (int i = 0; i < 6; i++)
            {
                if (!allEnableStates[i])
                {
                    await WriteSingleCoilAsync(_slaveAddress, enableAddresses[i], true, 取消令牌);
                    hasModifiedEnable = true;
                }
            }

            if (hasModifiedEnable)
                await Task.Delay(100, 取消令牌); // 等待硬件继电器响应与伺服驱动就绪

            // 4. 精准独立校验当前需要动作轴的目标定位是否越界
            int currentTarget = await 查询轴目标位置(轴编号, 取消令牌);
            int allowedMaxLimit = maxLimit - currentHomePos;
            if (currentTarget < 0 || currentTarget > allowedMaxLimit)
            {
                throw new ArgumentOutOfRangeException($"轴 {轴编号} 无法启动！当前指定目标位置 [{currentTarget}] 超过了允许的最大安全极限值 [{allowedMaxLimit}]。");
            }

            // 5. 校验完成，边缘触发点动启动
            await TriggerCoilAsync(startAddress, 取消令牌);
        }

        /// <summary>
        /// 三轴同时启动（全开：严格校验全部运行轴速度、批量检测与补正1~6号轴使能、联动校验三轴目标范围极限）
        /// </summary>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 启动全部轴(CancellationToken 取消令牌 = default)
        {
            // 1. 一次性读取三轴速度，严禁任何一轴速度为 0
            int xSpeed = await 查询轴速度(1, 取消令牌);
            int ySpeed = await 查询轴速度(2, 取消令牌);
            int zSpeed = await 查询轴速度(3, 取消令牌);

            if (xSpeed == 0 || ySpeed == 0 || zSpeed == 0)
                throw new InvalidOperationException($"无法全开启动！存在运行速度为 0 的轴。当前速度 -> X:{xSpeed}, Y:{ySpeed}, Z:{zSpeed}");

            // 2. 批量读取 1 ~ 6 号轴连续的使能状态线圈，极大地优化网络性能
            bool[] enableStates = await ReadCoilsAsync(_slaveAddress, ADDR_HM_AXIS1_ENABLE, 6, 取消令牌);
            if (enableStates == null || enableStates.Length < 6)
                throw new InvalidOperationException("从 PLC 读取 1~6 号轴使能状态失败。");

            bool hasModifiedEnable = false;
            ushort[] enableAddresses = { ADDR_HM_AXIS1_ENABLE, ADDR_HM_AXIS2_ENABLE, ADDR_HM_AXIS3_ENABLE, ADDR_HM_AXIS4_ENABLE, ADDR_HM_AXIS5_ENABLE, ADDR_HM_AXIS6_ENABLE };

            for (int i = 0; i < 6; i++)
            {
                if (!enableStates[i])
                {
                    await WriteSingleCoilAsync(_slaveAddress, enableAddresses[i], true, 取消令牌);
                    hasModifiedEnable = true;
                }
            }

            // 如果执行过使能补齐补齐写入，在循环外进行统一延时，保证PLC时序安全
            if (hasModifiedEnable)
                await Task.Delay(100, 取消令牌);

            // 3. 获取三轴原点及当前设定的目标指定位置
            var (xHome, yHome, zHome) = await 查询原点位置(取消令牌);

            int xTarget = await 查询轴目标位置(1, 取消令牌);
            int yTarget = await 查询轴目标位置(2, 取消令牌);
            int zTarget = await 查询轴目标位置(3, 取消令牌);

            await WriteSingleCoilAsync(_slaveAddress, ADDR_M_X_START, true, 取消令牌);
            await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Y_START, true, 取消令牌);
            await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Z_START, true, 取消令牌);

            await Task.Delay(100, 取消令牌); // 保持高电平状态以适配 PLC 扫描周期

            await WriteSingleCoilAsync(_slaveAddress, ADDR_M_X_START, false, 取消令牌);
            await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Y_START, false, 取消令牌);
            await WriteSingleCoilAsync(_slaveAddress, ADDR_M_Z_START, false, 取消令牌);
        }

        /// <summary>
        /// 三轴相对移动：以（初始绝对值 + 累计偏移 + 本次偏移）做越界校验，
        /// 清零绝对位置寄存器后将偏移量写入目标寄存器，启动三轴联动，
        /// 等待到达后清零目标寄存器并更新累计偏移。偏移量可为负值。
        /// 调用前必须先执行回零()完成初始化。
        /// </summary>
        /// <param name="X偏移量">X轴相对偏移量（可为负）</param>
        /// <param name="Y偏移量">Y轴相对偏移量（可为负）</param>
        /// <param name="Z偏移量">Z轴相对偏移量（可为负）</param>
        /// <param name="超时秒数">等待到达目标位置的最大超时时间（秒）</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 相对移动(int X偏移量, int Y偏移量, int Z偏移量, int 超时秒数 = 60, CancellationToken 取消令牌 = default)
        {
            if (!_isHomed)
                throw new InvalidOperationException("尚未完成回零初始化，请先调用 回零() 方法。");

            // 1. 越界校验：当前位置 + 本次偏移是否在合法范围
            int targetAbsX = _originX + X偏移量;
            int targetAbsY = _originY + Y偏移量;
            int targetAbsZ = _originZ + Z偏移量;

            if (targetAbsX > 50000)
                throw new ArgumentOutOfRangeException(nameof(X偏移量),
                    $"X轴越界：当前 {_originX} + 偏移 {X偏移量} = {targetAbsX}，最大范围 50000");

            if (targetAbsY > 50000)
                throw new ArgumentOutOfRangeException(nameof(Y偏移量),
                    $"Y轴越界：当前 {_originY} + 偏移 {Y偏移量} = {targetAbsY}，最大范围 50000");

            if (targetAbsZ > 20000)
                throw new ArgumentOutOfRangeException(nameof(Z偏移量),
                    $"Z轴越界：当前 {_originZ} + 偏移 {Z偏移量} = {targetAbsZ}，最大范围 20000");

            // 2. 将偏移量写入目标位置寄存器（ABS已在上次移动后清零，目标 = 偏移量）
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_X_TARGET, Int32ToUshorts(X偏移量), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Y_TARGET, Int32ToUshorts(Y偏移量), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_HD_Z_TARGET, Int32ToUshorts(Z偏移量), 取消令牌);

            // 3. 启动三轴联动
            await 启动全部轴(取消令牌);
            await 等待到达指定位置(超时秒数);
            await 清空绝对寄存器();

            // 5. 到达后更新当前位置（累加偏移）
            _originX += X偏移量;
            _originY += Y偏移量;
            _originZ += Z偏移量;

        
            

        }
        public async Task 清空绝对寄存器( CancellationToken 取消令牌 = default)
        {
            // 6. 清零 6 轴绝对位置寄存器
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS2_POS, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS3_POS, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS4_POS, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS5_POS, Int32ToUshorts(0), 取消令牌);
            await WriteMultipleRegistersAsync(_slaveAddress, ADDR_ABS_AXIS6_POS, Int32ToUshorts(0), 取消令牌);
        }
        #region 内部高低字工具转化方法

        /// <summary>
        /// 触发点动脉冲信号（置 1 后，等待 100ms 自动置 0 释放）
        /// </summary>
        private async Task TriggerCoilAsync(ushort address, CancellationToken ct = default)
        {
            await WriteSingleCoilAsync(_slaveAddress, address, true, ct);
            await Task.Delay(100, ct);
            await WriteSingleCoilAsync(_slaveAddress, address, false, ct);
        }

        /// <summary>
        /// 将 32 位整型数据转换为 Modbus 的 2 个 16 位无符号整数（低字在前 CDAB 格式）
        /// </summary>
        private ushort[] Int32ToUshorts(int value)
        {
            ushort lowWord = (ushort)(value & 0xFFFF);
            ushort highWord = (ushort)((value >> 16) & 0xFFFF);
            return new ushort[] { lowWord, highWord };
        }

        /// <summary>
        /// 将 Modbus 的 2 个 16 位无符号整数转换为 32 位整型（低字在前 CDAB 格式）
        /// </summary>
        private int UshortsToInt32(ushort[] registers)
        {
            uint lowWord = registers[0];
            uint highWord = registers[1];
            return (int)((highWord << 16) | lowWord);
        }

        #endregion

        #region 轴绝对位置读取（只读）

        /// <summary>
        /// 读取指定轴的绝对位置（相对原点的实时位置，只读）
        /// </summary>
        /// <param name="轴编号">轴编号 (1-6)</param>
        /// <param name="取消令牌">取消令牌</param>
        /// <returns>指定轴的绝对位置值</returns>
        public async Task<int> 查询轴绝对位置(int 轴编号, CancellationToken 取消令牌 = default)
        {
            ushort address = 轴编号 switch
            {
                1 => ADDR_ABS_AXIS1_POS,
                2 => ADDR_ABS_AXIS2_POS,
                3 => ADDR_ABS_AXIS3_POS,
                4 => ADDR_ABS_AXIS4_POS,
                5 => ADDR_ABS_AXIS5_POS,
                6 => ADDR_ABS_AXIS6_POS,
                _ => throw new ArgumentException("轴编号错误，仅支持 1-6", nameof(轴编号))
            };

            ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, address, 2, 取消令牌);
            if (registers == null || registers.Length < 2)
                throw new InvalidOperationException($"读取轴 {轴编号} 绝对位置失败。");

            return UshortsToInt32(registers);
        }

        /// <summary>
        /// 批量读取 1-6 轴的绝对位置（一次性读取 22 个寄存器，高效）
        /// </summary>
        /// <param name="取消令牌">取消令牌</param>
        /// <returns>返回 6 轴绝对位置元组</returns>
        public async Task<(int Axis1, int Axis2, int Axis3, int Axis4, int Axis5, int Axis6)> 查询全部轴绝对位置(CancellationToken 取消令牌 = default)
        {
            // 从 47232 开始连续读取 22 个寄存器（覆盖 47232-47253）
            ushort[] registers = await ReadHoldingRegistersAsync(_slaveAddress, ADDR_ABS_AXIS1_POS, 22, 取消令牌);
            if (registers == null || registers.Length < 22)
                throw new InvalidOperationException("批量读取 6 轴绝对位置失败，返回数据长度不足。");

            // 每轴间隔 4 个寄存器（2个数据 + 2个间隙），提取各轴数据
            int axis1 = UshortsToInt32(new ushort[] { registers[0], registers[1] });
            int axis2 = UshortsToInt32(new ushort[] { registers[4], registers[5] });
            int axis3 = UshortsToInt32(new ushort[] { registers[8], registers[9] });
            int axis4 = UshortsToInt32(new ushort[] { registers[12], registers[13] });
            int axis5 = UshortsToInt32(new ushort[] { registers[16], registers[17] });
            int axis6 = UshortsToInt32(new ushort[] { registers[20], registers[21] });

            return (axis1, axis2, axis3, axis4, axis5, axis6);
        }

        #endregion

        #region 位置到达判定（超时轮询）
        /// <summary>
        /// 轮询等待三轴绝对位置均回到机械零点（容差 ±10），超时则抛出异常
        /// </summary>
        /// <param name="超时秒数">最大等待时间（秒）</param>
        /// <param name="取消令牌">取消令牌</param>
        [Browsable(false)]
        public async Task 等待到达零点(int 超时秒数, CancellationToken 取消令牌 = default)
        {
            var deadline = DateTime.Now.AddSeconds(超时秒数);

            while (DateTime.Now < deadline)
            {
                取消令牌.ThrowIfCancellationRequested();

                var pos = await 查询全部轴绝对位置(取消令牌);
                if (Math.Abs(pos.Axis1) <= 10
                    && Math.Abs(pos.Axis6) <= 10
                    && Math.Abs(pos.Axis2) <= 10)
                    return;

                await Task.Delay(100, 取消令牌);
            }

            throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未到达机械零点");
        }

        /// <summary>
        /// 轮询等待三轴绝对位置到达指定的待定位置（容差 ±10），超时则抛出异常
        /// </summary>
        /// <param name="xTarget">X轴目标位置</param>
        /// <param name="yTarget">Y轴目标位置</param>
        /// <param name="zTarget">Z轴目标位置</param>
        /// <param name="超时秒数">最大等待时间（秒）</param>
        /// <param name="取消令牌">取消令牌</param>
        private async Task 等待到达待定位置(int xTarget, int yTarget, int zTarget, int 超时秒数, CancellationToken 取消令牌 = default)
        {
            var deadline = DateTime.Now.AddSeconds(超时秒数);

            while (DateTime.Now < deadline)
            {
                取消令牌.ThrowIfCancellationRequested();

                var pos = await 查询全部轴绝对位置(取消令牌);
                if (Math.Abs(pos.Axis1 - xTarget) <= 10
                    && Math.Abs(pos.Axis6 - yTarget) <= 10
                    && Math.Abs(pos.Axis2 - zTarget) <= 10)
                    return;

                await Task.Delay(100, 取消令牌);
            }

            throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未到达待定位置 (X:{xTarget}, Y:{yTarget}, Z:{zTarget})");
        }

        /// <summary>
        /// 轮询等待三轴绝对位置均到达各自设定的目标位置（容差 ±10），超时则抛出异常
        /// </summary>
        /// <param name="超时秒数">最大等待时间（秒）</param>
        /// <param name="取消令牌">取消令牌</param>
        public async Task 等待到达指定位置(int 超时秒数, CancellationToken 取消令牌 = default)
        {
            var targetX = await 查询轴目标位置(1, 取消令牌);
            var targetY = await 查询轴目标位置(2, 取消令牌);
            var targetZ = await 查询轴目标位置(3, 取消令牌);

            var deadline = DateTime.Now.AddSeconds(超时秒数);

            while (DateTime.Now < deadline)
            {
                取消令牌.ThrowIfCancellationRequested();

                var pos = await 查询全部轴绝对位置(取消令牌);
                if (Math.Abs(pos.Axis1 - targetX) <= 10
                    && Math.Abs(pos.Axis6 - targetY) <= 10
                    && Math.Abs(pos.Axis2 - targetZ) <= 10)
                    return;

                await Task.Delay(100, 取消令牌);
            }

            throw new TimeoutException($"等待 {超时秒数} 秒后，三轴仍未到达指定位置 (X:{targetX}, Y:{targetY}, Z:{targetZ})");
        }

        #endregion
    }
}