Display_D_HP/TeamAAS-VM/Core/Robots/XYZU_Robot.cs

using MahApps.Metro.Controls;
using Opc.Ua;
using Prism;
using Prism.Ioc;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.ComponentModel;
using System.Diagnostics;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using TeamAAS_VP.Core.PLCs;
using TeamAAS_VP.Enums;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TouchSocket.Core;
using TouchSocket.Sockets;
using static OpenCvSharp.ML.DTrees;
using static System.Windows.Forms.AxHost;

namespace TeamAAS_VP.Core.Robots
{
    /// <summary>
    /// Generic multi-axis robot that composes axis objects and uses PLC nodes configured in RobotInfo.PlcRobotParameter
    /// to perform coordinated moves (Go/Move/CalibMotion) for 3 or 4 axes.
    /// This implementation maps X/Y/Z/(U) axes by name and writes their position nodes then triggers ExecuteMove node.
    /// </summary>
    public class XYZU_Robot : IRobot, INotifyPropertyChanged
    {
        public event Action<Guid, object, ConnectedEventArgs> ConnectedEvent;
        public event Action<Guid, object, ClosedEventArgs> DisconnectedEvent;
        public event Action<Guid, object, ReceivedDataEventArgs> ReceivedEvent;
        public event Action<Guid, object, string> SendEvent;

        public XYZU_Robot(RobotInfo robot, OpcUaClientPLC pLC)
        {
            RobotInfo = robot;
            Name = robot.RobotName;
            Id = robot.Id;
            RobotNo = robot.RobotNo;
            RobotIp = robot.IP;
            RobotPort = robot.Port;
            ConnectType = robot.ConnectType;
            Terminator = robot.Terminator;
            DataEncoding = robot.DataEncoding;
            Brand = RobotBrand.XYZ_Platform;
            Plc = pLC;
            Plc.ConnectChangedEvent += Plc_ConnectChangedEvent;

            // build axis list from known RobotInfo.PlcRobotParameter nodes (best-effort)
            Axes = new List<Axis>();
            // X
            var ax = new Axis();
            ax.Name = "X";
            ax.Index = 1;
            ax.Command = robot.PlcRobotParameter.AxixList[0].Command;
            ax.Parameter = robot.PlcRobotParameter.AxixList[0].Parameter;
            ax.State = robot.PlcRobotParameter.AxixList[0].State;
            Axes.Add(ax);

            // Y
            var ay = new Axis();
            ay.Name = "Y";
            ay.Index = 2;
            ay.Command = robot.PlcRobotParameter.AxixList[1].Command;
            ay.Parameter = robot.PlcRobotParameter.AxixList[1].Parameter;
            ay.State = robot.PlcRobotParameter.AxixList[1].State;
            Axes.Add(ay);
            // Z
            var az = new Axis();
            az.Name = "Z";
            az.Index = 3;
            az.Command = robot.PlcRobotParameter.AxixList[2].Command;
            az.Parameter = robot.PlcRobotParameter.AxixList[2].Parameter;
            az.State = robot.PlcRobotParameter.AxixList[2].State;
            Axes.Add(az);
            // U optional
            if (robot.PlcRobotParameter.AxixList.Count >= 4)
            {
                var au = new Axis();
                au.Name = "U";
                au.Index = 4;
                au.Command = robot.PlcRobotParameter.AxixList[3].Command;
                au.Parameter = robot.PlcRobotParameter.AxixList[3].Parameter;
                au.State = robot.PlcRobotParameter.AxixList[3].State;
                Axes.Add(au);
                Brand = RobotBrand.XYZU_Platform;
            }

            //附加轴集合
            if(robot.PlcRobotParameter.AdditionalAxisList!=null && robot.PlcRobotParameter.AdditionalAxisList.Count>0)
            {
                AdditionalAxisList = new ObservableCollection<AxisExtended>();
                foreach(var addAxis in robot.PlcRobotParameter.AdditionalAxisList)
                {
                    AxisExtended axisExt = new AxisExtended()
                    {
                        Name = addAxis.Name,
                        Index = addAxis.Index,
                        Command = addAxis.Command,
                        Parameter = addAxis.Parameter,
                        State = addAxis.State
                    };
                    AdditionalAxisList.Add(axisExt);
                }
            }

            if (Plc.IsConnected)
            {
                try
                {
                    //所有轴的位置节点
                    var nodeIds = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode)).Select(a => a.State.ActPositionNode).ToArray();
                    List<string> positionNodes = new List<string>(nodeIds);
                    pLC.SubscribeNodes("AxisPosition", positionNodes, (res) =>
                    {
                        if (res.key != "AxisPosition") return;

                        for (int i = 0; i < Axes.Count; i++)
                        {
                            var axis = Axes[i];
                            if (axis.State.ActPositionNode.Contains(res.nodeId))
                            {
                                float v = Convert.ToSingle(res.value);
                                switch (axis.Name)
                                {
                                    case "X": CurrentPosition.X = v; break;
                                    case "Y": CurrentPosition.Y = v; break;
                                    case "Z": CurrentPosition.Z = v; break;
                                    case "U": CurrentPosition.U = v; break;
                                }
                            }
                        }
                    });

                    //所有附加轴的位置节点
                    if (AdditionalAxisList != null && AdditionalAxisList.Count > 0)
                    {
                        var addNodeIds = AdditionalAxisList
                            .Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode))
                            .Select(a => a.State.ActPositionNode)
                            .ToArray();
                        List<string> addPositionNodes = new List<string>(addNodeIds);
                        pLC.SubscribeNodes("AdditionalAxisPosition", addPositionNodes, (res) =>
                        {
                            if (res.key != "AdditionalAxisPosition") return;
                            for (int i = 0; i < AdditionalAxisList.Count; i++)
                            {
                                var axis = AdditionalAxisList[i];
                                if (axis.State.ActPositionNode.Contains(res.nodeId))
                                {
                                    axis.CurrentPosition = Convert.ToSingle(res.value);
                                }
                            }
                        });
                    }


                }
                catch (Exception)
                {

                }
            }
        }

        private void Plc_ConnectChangedEvent(object arg1, bool arg2)
        {
            CanExecute = arg2;
            if (arg2)
                ConnectedEvent?.Invoke(Id, this, null);
            else
                DisconnectedEvent?.Invoke(Id, this, null);
        }

        #region 属性
        public TcpClient TcpClient { get; private set; }

        public TcpService TcpService { get; private set; }

        public Guid Id { get; set; }

        public string Name { get; set; }

        /// <summary>
        /// 机器人编号
        /// </summary>
        public int RobotNo { get; set; }

        public int RobotPort { get; private set; }

        public string RobotIp { get; private set; }

        public TCPConnectType ConnectType { get; private set; }

        public Terminator Terminator { get; private set; }

        public DataEncoding DataEncoding { get; private set; }

        public bool IsConnected
        {
            get
            {
                if (Plc != null)
                {
                    return Plc.IsConnected;
                }
                else
                {
                    return false;
                }
            }
        }

        public int Timeout { get; set; } = 5000;

        private bool _CanExecute = true;

        public bool CanExecute
        {
            get { return _CanExecute; }
            set { SetProperty(ref _CanExecute, value); }
        }

        public int SelectedTool { get; private set; } = 0;

        public RobotBrand Brand { get; private set; }

        public OpcUaClientPLC Plc { get; private set; }

        public RobotInfo RobotInfo { get; private set; }

        public List<Axis> Axes { get; private set; }

        private ObservableCollection<AxisExtended> _AdditionalAxisList;
        public ObservableCollection<AxisExtended> AdditionalAxisList
        {
            get { return _AdditionalAxisList; }
            set { SetProperty(ref _AdditionalAxisList, value); }
        }

        /// <summary>
        /// 进入调试模式
        /// </summary>
        /// <returns></returns>
        public bool EnterDebugMode { get; set; }

        private RPoint _CurrentPosition = new RPoint();
        /// <summary>
        /// 当前位置
        /// </summary>
        public RPoint CurrentPosition
        {
            get { return _CurrentPosition; }
            set { SetProperty(ref _CurrentPosition, value); }
        }
        #endregion

        #region 连接
        public void Connect()
        {
            if (Plc.IsConnected)
            {
                StopMove();
                ConnectedEvent?.Invoke(Id, this, null);
            }
        }

        public Task ConnectAsync()
        {
            if (Plc.IsConnected)
            {
                StopMove();
                ConnectedEvent?.Invoke(Id, this, null);
            }
            return Task.CompletedTask;
        }

        public void Disconnect()
        {

        }

        public void Dispose()
        {

        }
        #endregion

        #region 控制
        public bool Reset() => true;
        public Task<bool> ResetAsync() { return Task.Run(() => Reset()); }
        public bool Motor(bool state)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.PowerNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = state;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return Plc.WriteNodes(nodesToWrite);
        }
        public async Task<bool> MotorAsync(bool state)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.PowerNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = state;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return await Plc.WriteNodesAsync(nodesToWrite);
        }
        public bool Power(bool state) => true;
        public Task<bool> PowerAsync(bool state) => Task.FromResult(true);
        public bool Speed(int value)
        {
            return true;
        }
        public Task<bool> SpeedAsync(int value) => Task.FromResult(true);
        public bool Speedfactor(int value) => true;
        public Task<bool> SpeedfactorAsync(int value) => Task.FromResult(true);
        public bool Speeds(double value)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = (float)value;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return Plc.WriteNodes(nodesToWrite);
        }
        public async Task<bool> SpeedsAsync(double value)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = (float)value;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return await Plc.WriteNodesAsync(nodesToWrite);
        }
        public bool Accel(int value) => true;
        public Task<bool> AccelAsync(int value) => Task.FromResult(true);
        public bool Accels(double value) => true;
        public Task<bool> AccelsAsync(double value) => Task.FromResult(true);

        public RPoint GetRobotPos()
        {
            if (Plc == null || !Plc.IsConnected) return null;
            try
            {
                var nodeIds = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode)).Select(a => a.State.ActPositionNode).ToArray();
                var data = Plc.ReadNodes(nodeIds);
                if (data == null || data.Count == 0) return null;

                //获取所有的值
                var values = data.Values.ToArray();

                RPoint point = new RPoint();
                for (int i = 0; i < Axes.Count && i < values.Length; i++)
                {
                    var val = values[i];
                    float v = 0;
                    if (val != null)
                    {
                        try { v = Convert.ToSingle(val); } catch { }
                    }
                    switch (Axes[i].Name)
                    {
                        case "X": point.X = v; break;
                        case "Y": point.Y = v; break;
                        case "Z": point.Z = v; break;
                        case "U": point.U = v; break;
                        default: break;
                    }
                }
                return point;
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("获取机器人位置出错", ex);
                throw;
            }
        }

        public Task<RPoint> GetRobotPosAsync()
        {
            return Task.Run(() => GetRobotPos());
        }

        public bool Go(RPoint position)
        {
            return WaitMoveFinished(position);
        }

        public Task<bool> GoAsync(RPoint position)
        {
            return Task.Run(() => Go(position));
        }

        public bool Move(RPoint position) => Go(position);
        public Task<bool> MoveAsync(RPoint position) => GoAsync(position);
        public bool Jump(RPoint position, double? LimZ)
        {
            return CalibMotion(position,LimZ);
        }
        public Task<bool> JumpAsync(RPoint position, double? LimZ) => CalibMotionAsync(position,LimZ);
        public bool Jog(string axis, double distance)
        {
            //获取当前机器人坐标
            var currentPos = GetRobotPos();
            if (currentPos == null) return false;
            switch (axis.ToUpper())
            {
                case "X":
                    currentPos.X += (float)distance;
                    break;
                case "Y":
                    currentPos.Y += (float)distance;
                    break;
                case "Z":
                    currentPos.Z += (float)distance;
                    break;
                case "U":
                    currentPos.U += (float)distance;
                    break;
                default:
                    return false;
            }
            return Go(currentPos);

        }
        public Task<bool> JogAsync(string axis, double distance)
        {
            return Task.Run(() => Jog(axis, distance));
        }
        public bool Joint(int joint, double distance)
        {
            return false;
        }
        public Task<bool> JointAsync(int joint, double distance) => Task.FromResult(false);
        public bool SFree()
        {
            return Motor(false);
        }
        public Task<bool> SFreeAsync() => Task.Run(() => SFree());
        public bool SLock() => Motor(true);
        public Task<bool> SLockAsync() => Task.Run(() => SLock());
        public bool CalibMotion(RPoint position, double? LimZ)
        {
            //先Z轴到安全高度
            var currentPos = GetRobotPos();
            if (currentPos == null) return false;
            if (LimZ.HasValue)
            {
                currentPos.Z = (float)LimZ.Value;
                if (!Go(currentPos)) return false;
            }
            //再XYU轴到位
            currentPos.X = position.X;
            currentPos.Y = position.Y;
            currentPos.U = position.U;
            if (!Go(currentPos)) return false;
            //最后Z轴到目标高度
            currentPos.Z = position.Z;
            var isSucceed = Go(currentPos);
            Thread.Sleep(150);
            return isSucceed;
        }
        public Task<bool> CalibMotionAsync(RPoint position, double? LimZ) => Task.Run(() => CalibMotion(position, LimZ));
        public bool CalibOutIO(bool state) => false;
        public Task<bool> CalibOutIOAsync(bool state) => Task.FromResult(false);
        public bool CalibParame(PickInfo Pick, int Speed, int Accel, bool Power, int WaitSuction, int WaitBlow)
        {
            return false;

        }

        public async Task<bool> CalibParameAsync(PickInfo Pick, int Speed, int Accel, bool Power, int WaitSuction, int WaitBlow)
        {
            if (Plc == null || !Plc.IsConnected) return false;
            try
            {
                Dictionary<string, object> keyValues = new Dictionary<string, object>();
                foreach (var axis in Axes)
                {
                    if (string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode)) continue;
                    string nodeid = axis.Parameter.ManuVelocityNode;
                    float value = Speed;
                    
                    keyValues.Add(nodeid, value);
                }
                // 2. 写入速度给所有轴
                if (!Plc.WriteNodes(keyValues))
                {
                    return false;
                }
                return true;
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("为XYZU平台设置速度时出错！", ex);
                return false;
            }
        }

        /// <summary>
        /// 阻塞式等待运动完成
        /// </summary>
        /// <param name="position"></param>
        /// <returns></returns>
        private bool WaitMoveFinished(RPoint position)
        {
            if (Plc == null || !Plc.IsConnected) return false;
            try
            {
                CanExecute = false;
                Motor(true);
                Dictionary<string, object> keyValues = new Dictionary<string, object>();

                // 1. 写入目标位置到各轴的 ManuPositionNode
                keyValues = new Dictionary<string, object>();
                foreach (var axis in Axes)
                {
                    if (string.IsNullOrWhiteSpace(axis.Parameter.ManuPositionNode)) continue;
                    string nodeid = axis.Parameter.ManuPositionNode;
                    float value = 0;
                    switch (axis.Name)
                    {
                        case "X": value = position.X; break;
                        case "Y": value = position.Y; break;
                        case "Z": value = position.Z; break;
                        case "U": value = position.U; break;
                        default: value = 0; break;
                    }
                    keyValues.Add(nodeid, value);
                }
                // 2. 写入位置给所有轴
                if (!Plc.WriteNodes(keyValues))
                {
                    return false;
                }

                // 3. 写入开始移动命令给所有轴
                if (!StartMove())
                {
                    return false;
                }

                // 准备停滞检测：收集所有实际位置节点
                var actNodes = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode))
                                   .Select(a => a.State.ActPositionNode)
                                   .Distinct()
                                   .ToArray();

                // lastPos 存储上一次读取到的位置，lastChange 存储上次发生“实质性”变化的时间
                Dictionary<string, float> lastPos = new Dictionary<string, float>();
                Dictionary<string, DateTime> lastChange = new Dictionary<string, DateTime>();
                foreach (var node in actNodes)
                {
                    lastPos[node] = float.NaN;
                    lastChange[node] = DateTime.UtcNow;
                }

                const float movementThreshold = 0.01f; // 判断位置变化的阈值，避免噪声
                Stopwatch sw = new Stopwatch();
                sw.Start();
                while (true)
                {
                    // 先读取各轴的实际位置，用于停滞检测
                    Dictionary<string, object> posRead = new Dictionary<string, object>();
                    try
                    {
                        if (actNodes.Length > 0)
                        {
                            posRead = Plc.ReadNodes(actNodes);
                        }
                    }
                    catch
                    {
                        // 如果读取失败，继续让 CheckMoveFinished 来处理状态或超时
                    }

                    var now = DateTime.UtcNow;

                    // 更新每个节点的变化时间：只要任意一个轴在最近 Timeout 时间内有变化，就视为系统仍在运动
                    foreach (var node in actNodes)
                    {
                        float actual = 0;
                        if (posRead != null && posRead.ContainsKey(node))
                        {
                            var raw = posRead[node];
                            if (raw != null)
                            {
                                try { actual = Convert.ToSingle(raw); } catch { /* 保持 actual = 0 */ }
                            }
                        }

                        if (float.IsNaN(lastPos[node]))
                        {
                            lastPos[node] = actual;
                            lastChange[node] = now;
                        }
                        else
                        {
                            if (Math.Abs(actual - lastPos[node]) > movementThreshold)
                            {
                                // 有实质性变化，更新记录时间和值
                                lastPos[node] = actual;
                                lastChange[node] = now;
                            }
                            // 否则保持 lastChange 不变（表示该轴最近一次变化的时间）
                        }
                    }

                    // 判断是否至少有一个轴在最近 Timeout 时间内发生过变化（即认为还在运动）
                    bool anyAxisMovedRecently = actNodes.Any(node => (now - lastChange[node]).TotalMilliseconds <= 1000);

                    // 如果没有任何轴在最近 Timeout 时间内发生变化，则认为出现停滞异常
                    if (!anyAxisMovedRecently && actNodes.Length > 0)
                    {
                        StopMove();
                        LogHelper.WriteLogInfo($"轴停滞超时（整体判定），目标位置：X={position.X},Y={position.Y},Z={position.Z},U={position.U}，超时阈值(ms)={Timeout}");
                        return false;
                    }

                    // 检查是否整体到位或有错误（保留原有逻辑）
                    var (isFinished, isError) = CheckMoveFinished(position);
                    if (isFinished)
                    {
                        StopMove();
                        if (isError)
                        {
                            LogHelper.WriteLogInfo($"机器人执行移动时发生错误，位置：X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
                            return false;
                        }
                        return true;
                    }

                    // 检查整体超时（防止一直有抖动导致 anyAxisMovedRecently 一直为 true）
                    if (sw.ElapsedMilliseconds > 60000) // 保守超时，一分钟，可根据需求调整
                    {
                        LogHelper.WriteLogInfo($"机器人执行移动超时，位置：X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
                        StopMove();
                        return false;
                    }

                    Thread.Sleep(10);
                }
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("执行XYZU平台，阻塞式等待运动完成时出错！", ex);
                CanExecute = true;
                return false;
            }
            finally
            {
                CanExecute = true;
            }
        }

        /// <summary>
        /// 开始运动
        /// </summary>
        /// <returns></returns>
        private bool StartMove()
        {
            Dictionary<string, object> keyValues = new Dictionary<string, object>();

            // 1. 复位所有轴的开始移动命令
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                {
                    keyValues.Add(axis.Command.ManuPositionNode, true);
                }
            }
            return Plc.WriteNodes(keyValues);
        }

        /// <summary>
        /// 停止运动
        /// </summary>
        /// <returns></returns>
        private bool StopMove()
        {
            Dictionary<string, object> keyValues = new Dictionary<string, object>();

            // 1. 复位所有轴的开始移动命令
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                {
                    keyValues.Add(axis.Command.ManuPositionNode, false);
                }
                if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
                {
                    keyValues.Add(axis.Command.StopNode, true);
                }
            }
            bool result = Plc.WriteNodes(keyValues);

            // 异步等待100ms后复位停止命令
            //Task.Run(async () =>
            //{
            //    await Task.Delay(100);
            Thread.Sleep(50);
            Dictionary<string, object> resetValues = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
                {
                    resetValues.Add(axis.Command.StopNode, false);
                }
            }
            Plc.WriteNodes(resetValues);
            //});
            return result;
        }

        /// <summary>
        /// 检查运动是否结束，返回是否停止、是否有错误
        /// </summary>
        /// <param name="position"></param>
        /// <returns></returns>
        private (bool isFinished, bool isError) CheckMoveFinished(RPoint position)
        {
            //检查各轴是否到位和目标位置一致
            //批量获取所有轴的定位状态和位置、是否错误
            var nodeIds = new List<string>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.State.PosOKNode))
                {
                    nodeIds.Add(axis.State.PosOKNode);
                }
                if (!string.IsNullOrWhiteSpace(axis.State.ActPositionNode))
                {
                    nodeIds.Add(axis.State.ActPositionNode);
                }
                if (!string.IsNullOrWhiteSpace(axis.State.ErrorNode))
                {
                    nodeIds.Add(axis.State.ErrorNode);
                }
                if (!string.IsNullOrWhiteSpace(axis.State.PausedNode))
                {
                    nodeIds.Add(axis.State.PausedNode);
                }
            }
            var res = Plc.ReadNodes(nodeIds.ToArray());
            bool allOk = true;
            bool isAnyError = false;
            foreach (var axis in Axes)
            {
                bool posOk = false;
                float actualPos = 0;
                bool isError = false;
                //获取此轴的定位状态、实际位置、错误状态
                if (res.ContainsKey(axis.State.PosOKNode))
                {
                    posOk = (bool)res[axis.State.PosOKNode];
                }
                if (res.ContainsKey(axis.State.ActPositionNode))
                {
                    try { actualPos = Convert.ToSingle(res[axis.State.ActPositionNode]); } catch { }
                }
                if (res.ContainsKey(axis.State.ErrorNode))
                {
                    isError = (bool)res[axis.State.ErrorNode];
                }
                if (res.ContainsKey(axis.State.PausedNode))
                {
                    isError = (bool)res[axis.State.PausedNode];
                }
                if (isError)
                {
                    isAnyError = true;
                    break;
                }
                //检查是否到位和位置一致
                float targetPos = 0;
                switch (axis.Name)
                {
                    case "X": targetPos = position.X; break;
                    case "Y": targetPos = position.Y; break;
                    case "Z": targetPos = position.Z; break;
                    case "U": targetPos = position.U; break;
                    default: targetPos = 0; break;
                }
                //如果定位完成且位置一致，则此轴完成，如果有错误则失败
                if (!(posOk && Math.Abs(actualPos - targetPos) < 0.02))
                {
                    if (isError)
                    {
                        isAnyError = true;
                        break;
                    }
                    allOk = false;
                    break;
                }
            }
            return (allOk, isAnyError);
        }

        /// <summary>
        /// 检查单个附加轴是否结束，返回是否停止、是否有错误
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        private (bool isFinished, bool isError) CheckAdditionalAxisMoveFinished(AxisExtended axis, double targetPos)
        {
            //读取实际位置和到位状态
            bool posOk = false;
            float actualPos = 0;
            bool isError = false;
            var res = Plc.ReadNodes(new string[] { axis.State.PosOKNode, axis.State.ActPositionNode, axis.State.ErrorNode, axis.State.PausedNode });
            if (res.ContainsKey(axis.State.PosOKNode))
            {
                posOk = (bool)res[axis.State.PosOKNode];
            }
            if (res.ContainsKey(axis.State.ActPositionNode))
            {
                try { actualPos = Convert.ToSingle(res[axis.State.ActPositionNode]); } catch { }
            }
            if (res.ContainsKey(axis.State.ErrorNode))
            {
                isError = (bool)res[axis.State.ErrorNode];
            }
            if (res.ContainsKey(axis.State.PausedNode))
            {
                isError = (bool)res[axis.State.PausedNode];
            }
            //检查是否到位和位置一致
            if (isError)
            {
                return (true, true);
            }
            if (posOk && Math.Abs(actualPos - targetPos) < 0.02)
            {
                return (true, false);
            }
            return (false, false);
        }

        /// <summary>
        /// 附加轴使能操作
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool AdditionalAxisMotor(string axisName, bool state)
        {
            var axis = AdditionalAxisList.FirstOrDefault(a => a.Name.Equals(axisName, StringComparison.OrdinalIgnoreCase));
            if (axis == null) return false;
            if (Plc == null || !Plc.IsConnected) return false;
            try
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.PowerNode))
                {
                    return Plc.WriteNode(axis.Command.PowerNode, state);
                }
                return false;
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("执行附加轴使能操作时出错！", ex);
                return false;
            }
        }

        //附加轴使能操作，异步方法
        public Task<bool> AdditionalAxisMotorAsync(string axisName, bool state)
        {
            return Task.Run(() => AdditionalAxisMotor(axisName, state));
        }

        /// <summary>
        /// 附加轴，阻塞式等待运动完成
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public bool AdditionalAxisMove(string axisName, double targetPos)
        {
            var axis = AdditionalAxisList.FirstOrDefault(a => a.Name.Equals(axisName, StringComparison.OrdinalIgnoreCase));
            if (axis == null) return false;
            if (Plc == null || !Plc.IsConnected) return false;
            try
            {
                CanExecute = false;
                AdditionalAxisMotor(axis.Name, true);
                //写入目标位置
                if (!string.IsNullOrWhiteSpace(axis.Parameter.ManuPositionNode))
                {
                    Plc.WriteNode(axis.Parameter.ManuPositionNode, (float)targetPos);
                }
                //开始移动
                if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                {
                    Plc.WriteNode(axis.Command.ManuPositionNode, true);
                }

                //准备停滞检测：收集所有实际位置节点
                float lastPos = float.NaN;
                DateTime lastChange = DateTime.UtcNow;
                const float movementThreshold = 0.01f; // 判断位置变化的阈值，避免噪声
                Stopwatch sw = new Stopwatch();
                sw.Start();
                
                while (true)
                {
                    var now = DateTime.UtcNow;
                    // 先读取轴的实际位置，用于停滞检测
                    var actual = Plc.ReadNode<float>(axis.State.ActPositionNode);
                    if (float.IsNaN(lastPos))
                    {
                        lastPos = actual;
                        lastChange = now;
                    }
                    else
                    {
                        if (Math.Abs(actual - lastPos) > movementThreshold)
                        {
                            // 有实质性变化，更新记录时间和值
                            lastPos = actual;
                            lastChange = now;
                        }
                        // 否则保持 lastChange 不变（表示该轴最近一次变化的时间）
                    }

                    // 判断是否至少有一个轴在最近 Timeout 时间内发生过变化（即认为还在运动）
                    bool anyAxisMovedRecently = (now - lastChange).TotalMilliseconds <= 1000;

                    // 如果没有任何轴在最近 Timeout 时间内发生变化，则认为出现停滞异常
                    if (!anyAxisMovedRecently)
                    {
                        //停止移动
                        if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                        {
                            Plc.WriteNode(axis.Command.ManuPositionNode, false);
                        }
                        return false;
                    }

                    // 检查是否整体到位或有错误（保留原有逻辑）
                    var (isFinished, isError) = CheckAdditionalAxisMoveFinished(axis, targetPos);
                    if (isFinished)
                    {
                        //停止移动
                        if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                        {
                            Plc.WriteNode(axis.Command.ManuPositionNode, false);
                        }
                        if (isError)
                        {
                            return false;
                        }
                        return true;
                    }

                    // 检查整体超时（防止一直有抖动导致 anyAxisMovedRecently 一直为 true）
                    if (sw.ElapsedMilliseconds > 60000) // 保守超时，一分钟，可根据需求调整
                    {
                        //停止移动
                        if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                        {
                            Plc.WriteNode(axis.Command.ManuPositionNode, false);
                        }
                        return false;
                    }

                    Thread.Sleep(10);
                }

            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("执行附加轴移动时出错！", ex);
                return false;
            }
            finally
            {
                CanExecute = true;
            }
        }

        /// <summary>
        /// 附加轴，阻塞式等待运动完成，异步方法
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        public Task<bool> AdditionalAxisMoveAsync(string axisName, double targetPos)
        {
            return Task.Run(() => AdditionalAxisMove(axisName, targetPos));
        }

        /// <summary>
        /// 附加轴Jog+点动
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="distance"></param>
        /// <returns></returns>
        public bool AdditionalAxisJog(string axisName, double distance)
        {
            var axis = AdditionalAxisList.FirstOrDefault(a => a.Name.Equals(axisName, StringComparison.OrdinalIgnoreCase));
            if (axis == null) return false;
            
            if (Plc == null || !Plc.IsConnected) return false;
            try
            {
                CanExecute = false;

                // 先读取轴的实际位置，用于停滞检测
                var actual = Plc.ReadNode<float>(axis.State.ActPositionNode);
                //计算目标位置
                double targetPos = actual + distance;

                AdditionalAxisMotor(axis.Name, true);

                return AdditionalAxisMove(axis.Name, targetPos);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("执行附加轴点动时出错！", ex);
                return false;
            }
            finally
            {
                CanExecute = true;
            }
        }

        /// <summary>
        /// 附加轴Jog-点动
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="distance"></param>
        /// <returns></returns>
        public bool AdditionalAxisJogNeg(string axisName, double distance)
        {
            return AdditionalAxisJog(axisName, -distance);
        }

        /// <summary>
        /// 附加轴异步 Jog+点动
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="distance"></param>
        /// <returns></returns>
        public Task<bool> AdditionalAxisJogAsync(string axisName, double distance)
        {
            return Task.Run(() => AdditionalAxisJog(axisName, distance));
        }

        /// <summary>
        /// 附加轴异步 Jog-点动
        /// </summary>
        /// <param name="axisName"></param>
        /// <param name="distance"></param>
        /// <returns></returns>
        public Task<bool> AdditionalAxisJogNegAsync(string axisName, double distance)
        {
            return Task.Run(() => AdditionalAxisJogNeg(axisName, distance));
        }

        #endregion

        #region 收发数据
        public string SendAndReceive(string send) => string.Empty;
        public Task<string> SendAndReceiveAsync(string send) => Task.FromResult(string.Empty);
        public string SendAndReceive(ITcpSessionClient client, string send) => string.Empty;
        public Task<string> SendAndReceiveAsync(ITcpSessionClient client, string send) => Task.FromResult(string.Empty);
        public void Send(string send) { }
        public Task SendAsync(string send) => Task.CompletedTask;
        public void Send(ITcpSessionClient client, string send) { }
        public Task SendAsync(ITcpSessionClient client, string send) => Task.CompletedTask;
        public Encoding GetEncoding() => Encoding.Default;
        #endregion

        #region 属性通知
        public event PropertyChangedEventHandler PropertyChanged;
        protected virtual bool SetProperty<T>(ref T storage, T value, [CallerMemberName] string propertyName = null)
        {
            if (EqualityComparer<T>.Default.Equals(storage, value)) return false;
            storage = value;
            OnPropertyChanged(new PropertyChangedEventArgs(propertyName));
            return true;
        }
        protected void OnPropertyChanged(PropertyChangedEventArgs args) => PropertyChanged?.Invoke(this, args);

        #endregion

        #region IRobot SetTool/SelectTool stubs
        public bool SetTool(int index, double x, double y) => true;
        public Task<bool> SetToolAsync(int index, double x, double y) => Task.FromResult(true);
        public bool SelectTool(int index) => true;
        public Task<bool> SelectToolAsync(int index) => Task.FromResult(true);
        #endregion
    }


}