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

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

namespace TeamAAS.Motion.Motions
{
    /// <summary>
    /// 主从跟随：主轴（虚轴）有独立规划位置；从轴只跟随主轴的相对位移。
    /// 主轴从 0→10，从轴当前 1 则走到 11。从轴可被多条主轴共用，互不影响各自主轴读数。
    /// </summary>
    public class CombinedMotionAxis : IAxis
    {
        private readonly IMotionCard _card;
        private readonly MotionAxisConfig _cfg;
        private readonly IAxis _master;
        private readonly Func<int, IAxis> _resolve;

        public CombinedMotionAxis(IMotionCard card, MotionAxisConfig cfg, IAxis master, Func<int, IAxis> resolve)
        {
            _card = card;
            _cfg = cfg ?? new MotionAxisConfig();
            _master = master;
            _resolve = resolve;
            AxisNo = _cfg.AxisNo;
            Name = string.IsNullOrWhiteSpace(_cfg.Name) ? $"Master-{AxisNo}" : _cfg.Name;
            Velocity = _cfg.Velocity;
            Acceleration = _cfg.Acceleration;
            Deceleration = _cfg.Deceleration;
            Jerk = _cfg.Jerk;
            if (_master != null)
            {
                _master.Name = Name;
                _master.Velocity = Velocity;
                _master.Acceleration = Acceleration;
                _master.Deceleration = Deceleration;
                _master.Jerk = Jerk;
            }
        }

        public int AxisNo { get; }
        public string Name { get; set; }
        public double Velocity { get; set; }
        public double Acceleration { get; set; }
        public double Deceleration { get; set; }
        public double Jerk { get; set; }

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

        public AxisState State => _master != null ? _master.State : AxisState.Off;
        public bool IsEnabled => _master != null && _master.IsEnabled;
        public bool IsMoving => _master != null && _master.IsMoving;
        public bool IsHomed => _master != null && _master.IsHomed;
        public double CommandPosition => _master != null ? _master.CommandPosition : 0;
        public double ActualPosition => _master != null ? _master.CommandPosition : 0;
        public double ActualVelocity => _master != null ? _master.ActualVelocity : 0;

        private IEnumerable<(IAxis Axis, double Scale)> Slaves()
        {
            foreach (var m in _cfg.ParseMembers())
            {
                if (m.AxisNo == AxisNo) continue;
                var a = _resolve(m.AxisNo);
                if (a != null && !ReferenceEquals(a, _master) && !(a is CombinedMotionAxis))
                    yield return (a, m.Scale);
            }
        }

        private void ApplyParams(IAxis axis)
        {
            if (axis == null) return;
            axis.Velocity = Velocity;
            axis.Acceleration = Acceleration;
            axis.Deceleration = Deceleration;
            axis.Jerk = Jerk;
        }

        private IMasterFollowHost FollowHost => _card as IMasterFollowHost;

        private IDisposable SoloFollow()
        {
            var host = FollowHost;
            var slaves = Slaves().Select(s => s.Axis).ToList();
            var scope = host?.SuspendFollowRelease();
            host?.BeginSoloMasterFollow(AxisNo, slaves);
            return scope ?? EmptyScope.Instance;
        }

        public void Enable()
        {
            if (_card is IMotionSafetyHost safety && !safety.AllowEnable(out var reason))
            {
                OnError(reason);
                return;
            }
            DriveMasterEnable(true);
        }

        public void Disable()
        {
            FollowHost?.ReleaseMasterFollow();
            DriveMasterEnable(false);
        }

        private MotionAxisConfig CfgOf(int axisNo)
        {
            if (_card is InovanceMotionCard imc) return imc.GetAxisConfig(axisNo);
            if (_card is SimulatedMotionCard sim) return sim.GetAxisConfig(axisNo);
            return axisNo == AxisNo ? _cfg : new MotionAxisConfig { AxisNo = axisNo };
        }

        public void MoveTo(double position)
        {
            if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
            if (_card is IMotionSafetyHost safety && !safety.AllowMotion(out var reason)) { OnError(reason); return; }
            if (_master == null) { OnError("主轴无效"); return; }
            var masterCfg = CfgOf(AxisNo);
            position = masterCfg.ClampPosition(position);
            Velocity = Math.Abs(masterCfg.ClampVelocity(Velocity));
            double delta = position - _master.CommandPosition;
            using (SoloFollow())
            {
                ApplyParams(_master);
                if (!DriveMasterMoveAbs(position)) { OnError("主轴定位失败"); return; }
                FollowRelative(delta);
            }
            MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, position, ActualPosition));
        }

        public void MoveBy(double distance)
        {
            if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
            if (_card is IMotionSafetyHost safety && !safety.AllowMotion(out var reason)) { OnError(reason); return; }
            if (_master == null) { OnError("主轴无效"); return; }
            var masterCfg = CfgOf(AxisNo);
            double target = masterCfg.ClampPosition(_master.CommandPosition + distance);
            Velocity = Math.Abs(masterCfg.ClampVelocity(Velocity));
            double delta = target - _master.CommandPosition;
            using (SoloFollow())
            {
                ApplyParams(_master);
                if (!DriveMasterMoveRel(delta)) { OnError("主轴相对定位失败"); return; }
                FollowRelative(delta);
            }
            MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, CommandPosition, ActualPosition));
        }

        public void Jog(double velocity)
        {
            if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
            if (_card is IMotionSafetyHost safety && !safety.AllowMotion(out var reason)) { OnError(reason); return; }
            if (_master == null) { OnError("主轴无效"); return; }
            velocity = CfgOf(AxisNo).ClampJog(velocity, _master.CommandPosition);
            using (SoloFollow())
            {
                ApplyParams(_master);
                if (Math.Abs(velocity) < 1e-9)
                {
                    DriveMasterStop();
                    return;
                }
                if (!DriveMasterJog(velocity)) { OnError("主轴点动失败"); return; }
                foreach (var s in Slaves())
                {
                    if (!s.Axis.IsEnabled) { OnError($"从轴 {s.Axis.Name} 未使能"); return; }
                    ApplyParams(s.Axis);
                    double sv = CfgOf(s.Axis.AxisNo).ClampJog(velocity * s.Scale, s.Axis.CommandPosition);
                    s.Axis.Jog(sv);
                }
            }
            MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, double.NaN, ActualPosition));
        }

        public void Stop()
        {
            DriveMasterStop();
            FollowHost?.ReleaseMasterFollow();
            MotionCompleted?.Invoke(this, new AxisMotionEventArgs(AxisNo, CommandPosition, ActualPosition));
        }

        public void Home(HomeMode mode = HomeMode.Default)
        {
            if (!_card.IsConnected) { OnError("运动卡未连接"); return; }
            if (_card is IMotionSafetyHost safety && !safety.AllowMotion(out var reason)) { OnError(reason); return; }
            if (_master == null) { OnError("主轴无效"); return; }
            double delta = -_master.CommandPosition;
            using (SoloFollow())
            {
                ApplyParams(_master);
                DriveMasterHome(mode);
                FollowRelative(delta);
            }
            MotionStarted?.Invoke(this, new AxisMotionEventArgs(AxisNo, 0, ActualPosition));
        }

        public void SetPosition(double position)
        {
            if (_master == null) return;
            if (_card is InovanceMotionCard imc)
                imc.SetCurrentPosition(AxisNo, position);
            else
                _master.SetPosition(position);
        }

        public void SetMotionParams(MotionParams parameters)
        {
            if (parameters == null) return;
            Velocity = parameters.Velocity;
            Acceleration = parameters.Acceleration;
            Deceleration = parameters.Deceleration;
            Jerk = parameters.Jerk;
            ApplyParams(_master);
        }

        private void FollowRelative(double masterDelta)
        {
            foreach (var s in Slaves())
            {
                if (!s.Axis.IsEnabled) { OnError($"从轴 {s.Axis.Name} 未使能"); return; }
                ApplyParams(s.Axis);
                double slaveDelta = masterDelta * s.Scale;
                double target = CfgOf(s.Axis.AxisNo).ClampPosition(s.Axis.CommandPosition + slaveDelta);
                s.Axis.MoveTo(target);
            }
        }

        private bool DriveMasterEnable(bool on)
        {
            if (_master == null) return false;
            if (_card is InovanceMotionCard imc)
                return imc.HardwareSetEnable(AxisNo, on);
            if (on) _master.Enable(); else _master.Disable();
            return true;
        }

        private bool DriveMasterMoveAbs(double position)
        {
            if (_card is InovanceMotionCard imc)
                return imc.HardwareMoveAbsolute(AxisNo, position, Velocity);
            _master.MoveTo(position);
            return true;
        }

        private bool DriveMasterMoveRel(double distance)
        {
            if (_card is InovanceMotionCard imc)
                return imc.HardwareMoveRelative(AxisNo, distance, Velocity);
            _master.MoveBy(distance);
            return true;
        }

        private bool DriveMasterJog(double velocity)
        {
            if (_card is InovanceMotionCard imc)
                return imc.HardwareJog(AxisNo, velocity);
            _master.Jog(velocity);
            return true;
        }

        private void DriveMasterStop()
        {
            if (_card is InovanceMotionCard imc)
                imc.HardwareStop(AxisNo);
            else
                _master?.Stop();
        }

        private void DriveMasterHome(HomeMode mode)
        {
            if (_card is InovanceMotionCard imc)
                imc.HardwareHome(AxisNo, (int)mode);
            else
                _master?.Home(mode);
        }

        private void OnError(string message)
            => ErrorOccurred?.Invoke(this, new AxisErrorEventArgs(AxisNo, message));

        private sealed class EmptyScope : IDisposable
        {
            public static readonly EmptyScope Instance = new EmptyScope();
            public void Dispose() { }
        }
    }
}