TeamAAS-2.5/TeamAAS.Core/TeamAAS.Motion/Motions/SimulatedMotionCard.cs

using HandyControl.Controls;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using TeamAAS.Motion.Interfaces;
using TeamAAS.Motion.Models;
using static TeamAAS.Motion.Enums.MotionEnums;
using static TeamAAS.Motion.Events.CardAxisEventArgs;

namespace TeamAAS.Motion.Motions
{
    /// <summary>
    /// 仿真运动卡：无硬件环境下的全功能实现（联调/演示/单元测试用）。
    /// 轴位置按指令速度渐变逼近目标（后台定时器推进），IO 点内存模拟。
    /// 品牌适配器（雷赛/固高/IMC60G）实现 IMotionCard 时可直接参照本类的状态机结构。
    /// </summary>
    public class SimulatedMotionCard : IMotionControl
    {
        private readonly MotionDeviceConfig _cfg;
        private readonly int _maxAxisCount;
        private readonly SimulatedAxis[] _axes;
        private readonly bool [] SimulatedDI=new bool[100];
        private readonly bool [] SimulatedDO=new bool[100];
        private Timer _updateTimer;
        private bool _isConnected = false;
        private readonly object _lock = new object();
        private const double UPDATE_INTERVAL_MS = 10; // 10ms 更新一次


        public string CardName => _cfg.Name;
        public bool IsConnected { get; private set; }

        public int MaxAxisCount => _cfg.AxisCount;


        public event EventHandler<ConnectionStateChangedEventArgs> ConnectionStateChanged;
        public event EventHandler<ErrorOccurredEventArgs> ErrorOccurred;

        public SimulatedMotionCard(MotionDeviceConfig cfg)
        {
            _cfg = cfg ?? new MotionDeviceConfig();
            _axes= new SimulatedAxis[ MaxAxisCount ];
            for ( int i = 0; i < MaxAxisCount; i++ )
                _axes[ i ] = new SimulatedAxis(i);

            for ( int i = 0; i < SimulatedDI.Length; i++ )
                SimulatedDI[ i ] = ( i % 2 == 0 );
 
        }


        protected virtual void OnConnectionStateChanged(ConnectionStateChangedEventArgs e)
        {
            ConnectionStateChanged?.Invoke(this, e);
        }

        protected virtual void OnErrorOccurred(ErrorOccurredEventArgs e)
        {
            ErrorOccurred?.Invoke(this, e);
        }
        public bool Connect(string connectionString)
        {
            lock ( _lock )
            {
                if ( _isConnected ) return true;
                // 模拟连接成功
                _isConnected = true;
                // 启动定时器更新轴状态
                _updateTimer = new Timer(UpdateAxes, null, 0, ( int ) UPDATE_INTERVAL_MS);
                OnConnectionStateChanged(new ConnectionStateChangedEventArgs(true));
                return true;
            }
        }

        public void Disconnect()
        {
            lock ( _lock )
            {
                if ( !_isConnected ) return;
                _isConnected = false;
                _updateTimer?.Dispose();
                _updateTimer = null;
                OnConnectionStateChanged(new ConnectionStateChangedEventArgs(false));
            }
        }

        public IAxis GetAxis(int axisNo)
        {
            if ( axisNo < 0 || axisNo >= _maxAxisCount )
                throw new ArgumentOutOfRangeException(nameof(axisNo), "轴号超出范围");
            return _axes[ axisNo ];
        }

        public IList<IAxis> GetAxes()
        {
            return _axes.Cast<IAxis>().ToList();
        }

        public void MoveLinear(double[] targetPositions, double velocity)
        {
            if ( !_isConnected ) throw new InvalidOperationException("控制卡未连接");
            if ( targetPositions.Length > _maxAxisCount )
                throw new ArgumentException("目标位置数组长度超过轴数");
            // 模拟：简单逐个轴移动（实际插补需协调）
            for ( int i = 0; i < targetPositions.Length; i++ )
            {
                var axis = _axes[i];
                if ( axis != null )
                    axis.MoveTo(targetPositions[ i ]);
            }
        }

        public void MoveArc(double[] centerPoint, double[] targetPositions, double velocity)
        {
            if ( !_isConnected ) throw new InvalidOperationException("控制卡未连接");
            if ( targetPositions.Length > _maxAxisCount )
                throw new ArgumentException("目标位置数组长度超过轴数");
            // 模拟：简单逐个轴移动（实际插补需协调）
            for ( int i = 0; i < targetPositions.Length; i++ )
            {
                var axis = _axes[i];
                if ( axis != null )
                    axis.MoveTo(targetPositions[ i ]);
            }
        }

        public void EmergencyStop()
        {
            if ( !_isConnected ) return;
            foreach ( var axis in _axes )
                axis.Stop();
        }

        public bool ReadDI(int portIndex)
        {
            return SimulatedDI[ portIndex ];
        }

        public void WriteDO(int portIndex, bool value)
        {
            SimulatedDO[ portIndex ] = value;
        }


        // 定时更新所有轴
        private void UpdateAxes(object state)
        {
            lock ( _lock )
            {
                if ( !_isConnected ) return;
                double deltaTime = UPDATE_INTERVAL_MS / 1000.0;
                foreach ( var axis in _axes )
                    axis.Update(deltaTime);
            }
        }

        public void Dispose()
        {
            Disconnect();
        }
    }


    public class SimulatedAxis : IAxis
    {
        private readonly object _lock = new object();
        private AxisState _state = AxisState.Off;
        private bool _isEnabled = false;
        private bool _isMoving = false;
        private bool _isHomed = false;
        private double _commandPos = 0.0;
        private double _actualPos = 0.0;
        private double _actualVel = 0.0;
        private double _targetPos = 0.0;
        private double _velocity = 100.0;   // 单位/秒
        private double _accel = 500.0;
        private double _decel = 500.0;
        private double _jerk = 1000.0;
        private double _jogVelocity = 0.0;   // 0表示不点动

        public int AxisNo { get; }
        public string Name { get; set; }

        public AxisState State => _state;
        public bool IsEnabled => _isEnabled;
        public bool IsMoving => _isMoving;
        public bool IsHomed => _isHomed;
        public double CommandPosition => _commandPos;
        public double ActualPosition => _actualPos;
        public double ActualVelocity => _actualVel;

        public double Velocity { get => _velocity; set => _velocity = value; }
        public double Acceleration { get => _accel; set => _accel = value; }
        public double Deceleration { get => _decel; set => _decel = value; }
        public double Jerk { get => _jerk; set => _jerk = value; }

        public event EventHandler<AxisMotionEventArgs> MotionStarted;
        public event EventHandler<AxisMotionEventArgs> MotionCompleted;
        public event EventHandler<AxisStateChangedEventArgs> StateChanged;
        public event EventHandler<AxisErrorEventArgs> ErrorOccurred;

        public SimulatedAxis(int axisNo)
        {
            AxisNo = axisNo;
            Name = $"Axis-{axisNo}";
        }

        // 由控制卡的定时器调用，更新轴状态（模拟位置变化）
        internal void Update(double deltaTime)
        {
            lock ( _lock )
            {
                if ( !_isEnabled || _state == AxisState.Error || _state == AxisState.Off )
                    return;

                // 如果处于点动状态（连续运动）
                if ( _state == AxisState.ContinuousMotion && Math.Abs(_jogVelocity) > 0.0001 )
                {
                    double delta = _jogVelocity * deltaTime;
                    _commandPos += delta;
                    _actualPos += delta;   // 无误差模拟
                    _actualVel = _jogVelocity;
                    return;
                }

                // 如果处于点位运动状态（离散运动）
                if ( _state == AxisState.DiscreteMotion )
                {
                    double remaining = _targetPos - _actualPos;
                    double distance = Math.Abs(remaining);
                    if ( distance < 0.0001 )
                    {
                        // 到达目标
                        _actualPos = _targetPos;
                        _commandPos = _targetPos;
                        _actualVel = 0;
                        SetState(AxisState.StandStill);
                        _isMoving = false;
                        OnMotionCompleted(new AxisMotionEventArgs(AxisNo, _targetPos, _actualPos));
                        return;
                    }

                    // 简单匀速运动（为模拟可加入加减速，但此处保持简单）
                    double speed = Math.Sign(remaining) * _velocity;
                    double step = speed * deltaTime;
                    // 防止过冲
                    if ( Math.Abs(step) > Math.Abs(remaining) )
                        step = remaining;
                    _actualPos += step;
                    _commandPos = _actualPos; // 指令位置跟随
                    _actualVel = step / deltaTime;
                }
            }
        }

        // 启动点位运动（内部调用）
        private void StartMoveTo(double target)
        {
            lock ( _lock )
            {
                if ( !_isEnabled )
                {
                    OnError("轴未使能，无法运动");
                    return;
                }
                if ( _state == AxisState.Error )
                {
                    OnError("轴处于错误状态");
                    return;
                }

                _targetPos = target;
                SetState(AxisState.DiscreteMotion);
                _isMoving = true;
                _jogVelocity = 0; // 取消点动
                OnMotionStarted(new AxisMotionEventArgs(AxisNo, target, _actualPos));
            }
        }

        #region IAxis 方法实现
        public void Enable()
        {
            lock ( _lock )
            {
                if ( _isEnabled ) return;
                _isEnabled = true;
                if ( _state == AxisState.Off ) SetState(AxisState.StandStill);
            }
        }

        public void Disable()
        {
            lock ( _lock )
            {
                if ( !_isEnabled ) return;
                // 停止运动
                _isMoving = false;
                _jogVelocity = 0;
                SetState(AxisState.Off);
                _isEnabled = false;
            }
        }

        public void MoveTo(double position)
        {
            StartMoveTo(position);
        }

        public void MoveBy(double distance)
        {
            double target = _actualPos + distance;
            StartMoveTo(target);
        }

        public void Jog(double velocity)
        {
            lock ( _lock )
            {
                if ( !_isEnabled )
                {
                    OnError("轴未使能，无法点动");
                    return;
                }
                if ( _state == AxisState.Error )
                {
                    OnError("轴处于错误状态");
                    return;
                }
                _jogVelocity = velocity;
                SetState(AxisState.ContinuousMotion);
                _isMoving = true;
                _actualVel = velocity;
                // 触发运动开始事件（带一个虚拟目标，因为点动无终点）
                OnMotionStarted(new AxisMotionEventArgs(AxisNo, double.NaN, _actualPos));
            }
        }

        public void Stop()
        {
            lock ( _lock )
            {
                if ( !_isMoving ) return;
                _jogVelocity = 0;
                if ( _state == AxisState.DiscreteMotion )
                {
                    // 对于点位运动，停止后当前位置即为最终位置（但未到达目标，触发停止完成事件？可以触发完成但标记未到位）
                    // 简单处理：立即停止，触发完成事件并报告未到达目标
                    _actualVel = 0;
                    SetState(AxisState.StandStill);
                    _isMoving = false;
                    OnMotionCompleted(new AxisMotionEventArgs(AxisNo, _targetPos, _actualPos));
                }
                else if ( _state == AxisState.ContinuousMotion )
                {
                    // 点动停止
                    _actualVel = 0;
                    SetState(AxisState.StandStill);
                    _isMoving = false;
                    OnMotionCompleted(new AxisMotionEventArgs(AxisNo, double.NaN, _actualPos));
                }
            }
        }

        public void Home(HomeMode mode = HomeMode.Default)
        {
            lock ( _lock )
            {
                if ( !_isEnabled )
                {
                    OnError("轴未使能，无法回零");
                    return;
                }
                // 模拟回零：将当前位置设为0
                _actualPos = 0;
                _commandPos = 0;
                _isHomed = true;
                SetState(AxisState.StandStill);
                // 触发回零完成事件？可以用MotionCompleted，但不设目标位置
                OnMotionCompleted(new AxisMotionEventArgs(AxisNo, 0, 0));
            }
        }

        public void SetPosition(double position)
        {
            lock ( _lock )
            {
                _actualPos = position;
                _commandPos = position;
            }
        }

        public void SetMotionParams(MotionParams parameters)
        {
            lock ( _lock )
            {
                if ( parameters != null )
                {
                    _velocity = parameters.Velocity;
                    _accel = parameters.Acceleration;
                    _decel = parameters.Deceleration;
                    _jerk = parameters.Jerk;
                }
            }
        }
        #endregion

        // 内部状态变更辅助
        private void SetState(AxisState newState)
        {
            if ( _state != newState )
            {
                var old = _state;
                _state = newState;
                OnStateChanged(new AxisStateChangedEventArgs(AxisNo, old, newState));
            }
        }

        // 事件触发辅助
        protected virtual void OnMotionStarted(AxisMotionEventArgs e) => MotionStarted?.Invoke(this, e);
        protected virtual void OnMotionCompleted(AxisMotionEventArgs e) => MotionCompleted?.Invoke(this, e);
        protected virtual void OnStateChanged(AxisStateChangedEventArgs e) => StateChanged?.Invoke(this, e);
        protected virtual void OnError(string message) => ErrorOccurred?.Invoke(this, new AxisErrorEventArgs(AxisNo, message));
    }

    /// <summary>仿真 IO 卡（16进/16出，内存模拟，批量接口齐全）。</summary>
    public class SimulatedIOCard : Global.Devices.IIOCard
    {
        private readonly MotionDeviceConfig _cfg;
        private readonly bool[] _inputs;
        private readonly bool[] _outputs;

        public string Name => _cfg.Name;
        public int InputCount => _inputs.Length;
        public int OutputCount => _outputs.Length;
        public bool IsConnected { get; private set; }

        public SimulatedIOCard(MotionDeviceConfig cfg)
        {
            _cfg = cfg ?? new MotionDeviceConfig();
            _inputs = new bool[ Math.Max(1, _cfg.InputCount) ];
            _outputs = new bool[ Math.Max(1, _cfg.OutputCount) ];
        }

        public bool Open() { IsConnected = true; return true; }
        public void Close() { IsConnected = false; }

        public bool[] ReadAllInputs()
        {
            lock ( _inputs ) { return ( bool[] ) _inputs.Clone(); }
        }

        public bool[] ReadAllOutputs()
        {
            lock ( _outputs ) { return ( bool[] ) _outputs.Clone(); }
        }

        public bool WriteOutput(int index, bool value)
        {
            if ( index < 0 || index >= _outputs.Length ) return false;
            lock ( _outputs ) { _outputs[ index ] = value; }
            return true;
        }

        public bool WriteOutputs(bool[] values)
        {
            if ( values == null ) return false;
            lock ( _outputs )
            {
                int n = Math.Min(values.Length, _outputs.Length);
                for ( int i = 0; i < n; i++ ) _outputs[ i ] = values[ i ];
            }
            return true;
        }
    }
}