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

using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using TeamAAS.Global.Devices;
using TeamAAS.Motion.Models;

namespace TeamAAS.Motion.Motions
{
    /// <summary>
    /// 仿真运动卡：无硬件环境下的全功能实现（联调/演示/单元测试用）。
    /// </summary>
    public class SimulatedMotionCard : IMotionCard
    {
        private readonly MotionDeviceConfig _cfg;
        private readonly int _axisCount;
        private readonly SimulatedAxis[] _axes;
        private readonly bool[] _simulatedDi = new bool[100];
        private readonly bool[] _simulatedDo = new bool[100];
        private Timer _updateTimer;
        private bool _isConnected;
        private readonly object _lock = new object();
        private const double UpdateIntervalMs = 10;

        private static readonly string[] DefaultAxisNames = { "X", "Y", "Z", "R" };

        public string Name => _cfg.Name;
        public bool IsConnected => _isConnected;
        public int AxisCount => _axisCount;

        public event EventHandler<MotionConnectionStateChangedEventArgs> ConnectionStateChanged;
        public event EventHandler<MotionErrorEventArgs> ErrorOccurred;

        public SimulatedMotionCard(MotionDeviceConfig cfg)
        {
            _cfg = cfg ?? new MotionDeviceConfig();
            _axisCount = Math.Max(1, _cfg.AxisCount);
            _axes = new SimulatedAxis[_axisCount];
            for (int i = 0; i < _axisCount; i++)
            {
                var axis = new SimulatedAxis(i);
                axis.Name = i < DefaultAxisNames.Length ? DefaultAxisNames[i] : $"Axis-{i}";
                _axes[i] = axis;
            }

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

        public bool Open()
        {
            lock (_lock)
            {
                if (_isConnected) return true;
                _isConnected = true;
                _updateTimer = new Timer(UpdateAxes, null, Timeout.Infinite, Timeout.Infinite);
            }
            _updateTimer.Change(0, (int)UpdateIntervalMs);
            ConnectionStateChanged?.Invoke(this, new MotionConnectionStateChangedEventArgs(true));
            return true;
        }

        public void Close()
        {
            Timer timerToDispose = null;
            lock (_lock)
            {
                if (!_isConnected) return;
                _isConnected = false;
                timerToDispose = _updateTimer;
                _updateTimer = null;
            }
            timerToDispose?.Dispose();
            ConnectionStateChanged?.Invoke(this, new MotionConnectionStateChangedEventArgs(false));
        }

        public bool SetEnable(int axis, bool enable)
        {
            if (!_isConnected) return false;
            if (axis < 0)
            {
                foreach (var a in _axes) { if (enable) a.Enable(); else a.Disable(); }
                return true;
            }
            if (!IsValidAxis(axis)) return false;
            if (enable) _axes[axis].Enable(); else _axes[axis].Disable();
            return true;
        }

        public bool Home(int axis, int mode = 0)
        {
            if (!_isConnected || !IsValidAxis(axis)) return false;
            _axes[axis].Home((HomeMode)mode);
            return true;
        }

        public bool IsHomed(int axis)
        {
            if (!IsValidAxis(axis)) return false;
            return _axes[axis].IsHomed;
        }

        public bool MoveAbsolute(int axis, double position, double velocity)
        {
            if (!_isConnected || !IsValidAxis(axis)) return false;
            _axes[axis].Velocity = velocity;
            _axes[axis].MoveTo(position);
            return true;
        }

        public bool MoveRelative(int axis, double distance, double velocity)
        {
            if (!_isConnected || !IsValidAxis(axis)) return false;
            _axes[axis].Velocity = velocity;
            _axes[axis].MoveBy(distance);
            return true;
        }

        public bool JogStart(int axis, int direction, double velocity)
        {
            if (!_isConnected || !IsValidAxis(axis)) return false;
            var dir = direction >= 0 ? 1 : -1;
            _axes[axis].Jog(dir * Math.Abs(velocity));
            return true;
        }

        public bool JogStop(int axis)
        {
            if (!IsValidAxis(axis)) return false;
            _axes[axis].Stop();
            return true;
        }

        public bool Stop(int axis = -1)
        {
            if (!_isConnected) return false;
            if (axis < 0)
            {
                foreach (var a in _axes) a.Stop();
                return true;
            }
            if (!IsValidAxis(axis)) return false;
            _axes[axis].Stop();
            return true;
        }

        public AxisStatus GetAxisStatus(int axis)
        {
            if (!IsValidAxis(axis)) return null;
            var a = _axes[axis];
            return new AxisStatus
            {
                Index = axis,
                Name = a.Name,
                Position = a.ActualPosition,
                Velocity = a.ActualVelocity,
                IsMoving = a.IsMoving,
                IsHomed = a.IsHomed,
                IsEnabled = a.IsEnabled
            };
        }

        public List<AxisStatus> GetAllAxisStatus()
        {
            var list = new List<AxisStatus>(_axisCount);
            for (int i = 0; i < _axisCount; i++)
                list.Add(GetAxisStatus(i));
            return list;
        }

        public IAxis GetAxis(int axisNo)
        {
            if (!IsValidAxis(axisNo))
                throw new ArgumentOutOfRangeException(nameof(axisNo), "轴号超出范围");
            return _axes[axisNo];
        }

        public IList<IAxis> GetAxes() => _axes.Cast<IAxis>().ToList();

        public bool MoveLinear(double[] targetPositions, double velocity)
        {
            if (!_isConnected || targetPositions == null) return false;
            if (targetPositions.Length > _axisCount) return false;
            for (int i = 0; i < targetPositions.Length; i++)
            {
                _axes[i].Velocity = velocity;
                _axes[i].MoveTo(targetPositions[i]);
            }
            return true;
        }

        public bool MoveArc(double[] centerPoint, double[] targetPositions, double velocity)
        {
            if (!_isConnected || targetPositions == null) return false;
            if (targetPositions.Length > _axisCount) return false;
            for (int i = 0; i < targetPositions.Length; i++)
            {
                _axes[i].Velocity = velocity;
                _axes[i].MoveTo(targetPositions[i]);
            }
            return true;
        }

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

        public bool ReadDI(int portIndex)
        {
            if (portIndex < 0 || portIndex >= _simulatedDi.Length) return false;
            return _simulatedDi[portIndex];
        }

        public bool WriteDO(int portIndex, bool value)
        {
            if (portIndex < 0 || portIndex >= _simulatedDo.Length) return false;
            _simulatedDo[portIndex] = value;
            return true;
        }

        private bool IsValidAxis(int axis) => axis >= 0 && axis < _axisCount;

        private void UpdateAxes(object state)
        {
            lock (_lock)
            {
                if (!_isConnected) return;
                double deltaTime = UpdateIntervalMs / 1000.0;
                foreach (var axis in _axes)
                    axis.Update(deltaTime);
            }
        }
    }

    public class SimulatedAxis : IAxis
    {
        private readonly object _lock = new object();
        private AxisState _state = AxisState.Off;
        private bool _isEnabled;
        private bool _isMoving;
        private bool _isHomed;
        private double _commandPos;
        private double _actualPos;
        private double _actualVel;
        private double _targetPos;
        private double _velocity = 100.0;
        private double _accel = 500.0;
        private double _decel = 500.0;
        private double _jerk = 1000.0;
        private double _jogVelocity;

        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));
            }
        }

        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) => StartMoveTo(_actualPos + distance);

        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;
                _actualVel = 0;
                SetState(AxisState.StandStill);
                _isMoving = false;
                OnMotionCompleted(new AxisMotionEventArgs(AxisNo, _targetPos, _actualPos));
            }
        }

        public void Home(HomeMode mode = HomeMode.Default)
        {
            lock (_lock)
            {
                if (!_isEnabled) { OnError("轴未使能，无法回零"); return; }
                _isMoving = false;
                _jogVelocity = 0;
                _actualVel = 0;
                _actualPos = 0;
                _commandPos = 0;
                _targetPos = 0;
                _isHomed = true;
                SetState(AxisState.StandStill);
                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) return;
                _velocity = parameters.Velocity;
                _accel = parameters.Acceleration;
                _decel = parameters.Deceleration;
                _jerk = parameters.Jerk;
            }
        }

        private void SetState(AxisState newState)
        {
            if (_state == newState) return;
            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));
    }

    public class SimulatedIOCard : 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;
        }
    }
}