ParkingRobot/MultiWheelC/Fleet/FleetMemberAgent.cs

using System;
using MyParking.Shared;

namespace MultiWheelC.Fleet
{
    /// <summary>表示成员车在一次车队动作中的本地执行阶段。</summary>
    public enum FleetMemberAgentState
    {
        Idle = 0,
        Preparing = 1,
        Ready = 2,
        Active = 3,
        Faulted = 4
    }

    /// <summary>区分固定β滚动运动和车辆中心纯自转的准备方式。</summary>
    public enum FleetMemberPreparationMode
    {
        Rolling = 0,
        Spin = 1
    }

    /// <summary>负责一辆成员车的舵轮准备、激活和本车速度命令执行。</summary>
    public sealed class FleetMemberAgent
    {
        private const double MotionDeadband = 1e-6;

        private readonly MultiWheelChassisAdapter _adapter;
        private readonly double _alignmentToleranceRadians;
        private readonly double _alignmentStableSeconds;

        private double _alignedDurationSeconds;
        private double? _lastAcceptedCommandTimeSeconds;
        private double? _commandDeadlineSeconds;

        /// <summary>创建绑定到一辆多舵轮底盘的成员车执行器。</summary>
        public FleetMemberAgent(
            MultiWheelChassisAdapter adapter,
            double alignmentToleranceRadians,
            double alignmentStableSeconds)
        {
            _adapter = adapter ??
                throw new ArgumentNullException(nameof(adapter));
            NumericGuard.EnsureFinitePositive(
                alignmentToleranceRadians,
                nameof(alignmentToleranceRadians));
            NumericGuard.EnsureFiniteNonNegative(
                alignmentStableSeconds,
                nameof(alignmentStableSeconds));

            if (alignmentToleranceRadians > Math.PI)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(alignmentToleranceRadians),
                    "舵轮到位容差不能大于π。");
            }

            _alignmentToleranceRadians =
                alignmentToleranceRadians;
            _alignmentStableSeconds =
                alignmentStableSeconds;
            State = FleetMemberAgentState.Idle;
            LastFailureReason = string.Empty;
        }

        public int VehicleId => _adapter.VehicleId;

        public FleetMemberAgentState State { get; private set; }

        public FleetMemberPreparationMode? PreparationMode
        {
            get;
            private set;
        }

        public long CurrentPlanId { get; private set; }

        public double MotionDirectionInBodyRadians
        {
            get;
            private set;
        }

        public string LastFailureReason { get; private set; }

        // 使用从车本机单调时钟记录，不依赖主车或Detour时间戳。
        public double? LastAcceptedCommandTimeSeconds =>
            _lastAcceptedCommandTimeSeconds;

        public double? CommandDeadlineSeconds =>
            _commandDeadlineSeconds;

        /// <summary>停车并开始准备本车固定β滚动运动系。</summary>
        public bool BeginRollingPreparation(
            long planId,
            double motionDirectionInBodyRadians)
        {
            ValidatePlanId(planId);
            NumericGuard.EnsureFinite(
                motionDirectionInBodyRadians,
                nameof(motionDirectionInBodyRadians));

            return BeginPreparation(
                planId,
                FleetMemberPreparationMode.Rolling,
                AngleMath.NormalizeRadians(
                    motionDirectionInBodyRadians));
        }

        /// <summary>停车并开始准备车辆中心纯自转所需的舵轮方向。</summary>
        public bool BeginSpinPreparation(long planId)
        {
            ValidatePlanId(planId);
            return BeginPreparation(
                planId,
                FleetMemberPreparationMode.Spin,
                motionDirectionInBodyRadians: 0.0);
        }

        /// <summary>检查舵轮是否已连续稳定到位；宿主应在准备阶段周期调用。</summary>
        public FleetMemberAgentState UpdatePreparation(
            double deltaTimeSeconds)
        {
            NumericGuard.EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            if (State != FleetMemberAgentState.Preparing)
            {
                return State;
            }

            bool aligned;
            try
            {
                aligned = UpdateAndCheckAlignment();
            }
            catch (InvalidOperationException exception)
            {
                Fail(exception.Message);
                return State;
            }
            catch (ArgumentException exception)
            {
                Fail(exception.Message);
                return State;
            }

            if (State == FleetMemberAgentState.Faulted)
            {
                return State;
            }

            _alignedDurationSeconds = aligned
                ? _alignedDurationSeconds + deltaTimeSeconds
                : 0.0;

            if (aligned &&
                _alignedDurationSeconds >=
                _alignmentStableSeconds)
            {
                State = FleetMemberAgentState.Ready;
                LastFailureReason = string.Empty;
            }

            return State;
        }

        /// <summary>在主车确认全队Ready后激活本车已经准备好的运动方式。</summary>
        public bool Activate(
            long planId,
            double commandReceivedTimeSeconds,
            double validForSeconds)
        {
            NumericGuard.EnsureFiniteNonNegative(
                commandReceivedTimeSeconds,
                nameof(commandReceivedTimeSeconds));
            NumericGuard.EnsureFinitePositive(
                validForSeconds,
                nameof(validForSeconds));

            if (planId != CurrentPlanId)
            {
                LastFailureReason =
                    "激活任务编号与当前准备任务不一致。";
                return false;
            }

            if (State == FleetMemberAgentState.Active)
            {
                // 重复激活只允许幂等确认，不能替代周期运动命令延长车辆运动时间。
                return UpdateCommandWatchdog(
                    commandReceivedTimeSeconds);
            }

            if (State != FleetMemberAgentState.Ready ||
                !PreparationMode.HasValue)
            {
                return RejectWhileStopped(
                    "成员车尚未完成舵轮准备。");
            }

            bool stillAligned;
            try
            {
                stillAligned = ArePreparedWheelsStillAligned();
            }
            catch (InvalidOperationException exception)
            {
                return Fail(exception.Message);
            }
            catch (ArgumentException exception)
            {
                return Fail(exception.Message);
            }

            if (!stillAligned)
            {
                State = FleetMemberAgentState.Preparing;
                _alignedDurationSeconds = 0.0;
                return RejectWhileStopped(
                    "成员车在激活前失去舵轮到位状态。");
            }

            try
            {
                if (PreparationMode.Value ==
                    FleetMemberPreparationMode.Rolling)
                {
                    _adapter.ActivateMotionFrame(
                        MotionDirectionInBodyRadians);
                }
                else if (!_adapter.AdoptPreparedSpinForXYTh(
                             _alignmentToleranceRadians))
                {
                    return Fail(
                        BuildAdapterFailureReason(
                            "无法激活已经准备好的原地自转舵轮。"));
                }
            }
            catch (InvalidOperationException exception)
            {
                return Fail(exception.Message);
            }
            catch (ArgumentException exception)
            {
                return Fail(exception.Message);
            }

            State = FleetMemberAgentState.Active;
            AcceptCommandDeadline(
                commandReceivedTimeSeconds,
                validForSeconds);
            LastFailureReason = string.Empty;
            return true;
        }

        /// <summary>校验任务和车号后执行分配给本车的车体系速度命令。</summary>
        public bool Execute(
            long planId,
            FleetMemberCommand command,
            double commandReceivedTimeSeconds,
            double validForSeconds,
            TimeSpan? interval = null)
        {
            ValidatePlanId(planId);
            NumericGuard.EnsureFinite(
                command.TwistInVehicleBody,
                nameof(command));
            NumericGuard.EnsureFiniteNonNegative(
                commandReceivedTimeSeconds,
                nameof(commandReceivedTimeSeconds));
            NumericGuard.EnsureFinitePositive(
                validForSeconds,
                nameof(validForSeconds));

            if (planId != CurrentPlanId)
            {
                return Fail(
                    "速度命令任务编号与当前激活任务不一致。");
            }

            if (command.VehicleId != VehicleId)
            {
                return Fail(
                    $"速度命令属于车辆{command.VehicleId}，" +
                    $"当前成员车号为{VehicleId}。");
            }

            if (State != FleetMemberAgentState.Active ||
                !PreparationMode.HasValue)
            {
                return RejectWhileStopped(
                    "成员车尚未激活，不能执行速度命令。");
            }

            // 先检查上一条命令是否已经过期，禁止失联后由迟到命令自动恢复运动。
            if (!UpdateCommandWatchdog(
                    commandReceivedTimeSeconds))
            {
                return false;
            }

            if (!IsCommandCompatibleWithPreparation(
                    command.TwistInVehicleBody))
            {
                return Fail(
                    "速度命令与本次舵轮准备方式不一致。");
            }

            try
            {
                if (!_adapter.SendBodyTwist(
                        command.TwistInVehicleBody,
                        interval))
                {
                    return Fail(
                        BuildAdapterFailureReason(
                            "成员车底盘拒绝执行速度命令。"));
                }
            }
            catch (InvalidOperationException exception)
            {
                return Fail(exception.Message);
            }
            catch (ArgumentException exception)
            {
                return Fail(exception.Message);
            }

            AcceptCommandDeadline(
                commandReceivedTimeSeconds,
                validForSeconds);
            LastFailureReason = string.Empty;
            return true;
        }

        // 运行循环即使没有收到新命令也必须调用本方法，超时后会本地停车并锁存Faulted。
        public bool UpdateCommandWatchdog(
            double currentTimeSeconds)
        {
            NumericGuard.EnsureFiniteNonNegative(
                currentTimeSeconds,
                nameof(currentTimeSeconds));

            if (State == FleetMemberAgentState.Faulted)
            {
                return false;
            }

            if (State != FleetMemberAgentState.Active)
            {
                return true;
            }

            if (!_lastAcceptedCommandTimeSeconds.HasValue ||
                !_commandDeadlineSeconds.HasValue)
            {
                return Fail(
                    "成员车已经激活，但本地命令看门狗尚未初始化。");
            }

            if (currentTimeSeconds <
                _lastAcceptedCommandTimeSeconds.Value)
            {
                return Fail(
                    "成员车本地单调时钟发生倒退，无法继续校验命令时效。");
            }

            if (currentTimeSeconds <=
                _commandDeadlineSeconds.Value)
            {
                return true;
            }

            var commandAgeSeconds =
                currentTimeSeconds -
                _lastAcceptedCommandTimeSeconds.Value;
            return Fail(
                "成员车等待主车有效命令超时，" +
                $"最近一次命令距今{commandAgeSeconds:F3}s。");
        }

        /// <summary>正常取消当前任务并立即停止驱动轮。</summary>
        public void Stop()
        {
            _adapter.StopImmediately();
            State = FleetMemberAgentState.Idle;
            PreparationMode = null;
            CurrentPlanId = 0;
            MotionDirectionInBodyRadians = 0.0;
            _alignedDurationSeconds = 0.0;
            ClearCommandWatchdog();
            LastFailureReason = string.Empty;
        }

        /// <summary>重置上一动作并下发本次滚动或自转舵轮准备目标。</summary>
        private bool BeginPreparation(
            long planId,
            FleetMemberPreparationMode mode,
            double motionDirectionInBodyRadians)
        {
            try
            {
                _adapter.StopImmediately();
                _adapter.ResetToBodyFrame();

                CurrentPlanId = planId;
                PreparationMode = mode;
                MotionDirectionInBodyRadians =
                    motionDirectionInBodyRadians;
                State = FleetMemberAgentState.Preparing;
                LastFailureReason = string.Empty;
                _alignedDurationSeconds = 0.0;
                ClearCommandWatchdog();

                var accepted = mode ==
                    FleetMemberPreparationMode.Rolling
                        ? _adapter.PrepareParallelDirection(
                            motionDirectionInBodyRadians)
                        : _adapter.PrepareSpin(
                            alignmentToleranceDegrees:
                                AngleMath.RadiansToDegrees(
                                    _alignmentToleranceRadians));

                if (!accepted)
                {
                    return Fail(
                        BuildAdapterFailureReason(
                            "成员车底盘拒绝舵轮准备目标。"));
                }

                return true;
            }
            catch (InvalidOperationException exception)
            {
                return Fail(exception.Message);
            }
            catch (ArgumentException exception)
            {
                return Fail(exception.Message);
            }
        }

        /// <summary>更新当前准备目标并读取舵轮到位状态。</summary>
        private bool UpdateAndCheckAlignment()
        {
            if (PreparationMode ==
                FleetMemberPreparationMode.Rolling)
            {
                return _adapter.AreParallelWheelsAligned(
                    MotionDirectionInBodyRadians,
                    _alignmentToleranceRadians);
            }

            if (!_adapter.PrepareSpin(
                    alignmentToleranceDegrees:
                        AngleMath.RadiansToDegrees(
                            _alignmentToleranceRadians)))
            {
                Fail(
                    BuildAdapterFailureReason(
                        "成员车底盘无法继续更新原地自转准备。"));
                return false;
            }

            return _adapter.AreSpinWheelsAligned;
        }

        /// <summary>确认舵轮在全队释放前仍保持到位。</summary>
        private bool ArePreparedWheelsStillAligned()
        {
            return PreparationMode ==
                   FleetMemberPreparationMode.Rolling
                ? _adapter.AreParallelWheelsAligned(
                    MotionDirectionInBodyRadians,
                    _alignmentToleranceRadians)
                : _adapter.AreSpinWheelsAligned;
        }

        /// <summary>禁止滚动准备执行纯自转，也禁止自转准备执行平移。</summary>
        private bool IsCommandCompatibleWithPreparation(
            Twist2D bodyTwist)
        {
            var linearSpeed = Math.Sqrt(
                bodyTwist.VxMetersPerSecond *
                bodyTwist.VxMetersPerSecond +
                bodyTwist.VyMetersPerSecond *
                bodyTwist.VyMetersPerSecond);
            var hasLinearMotion =
                linearSpeed > MotionDeadband;
            var hasAngularMotion =
                Math.Abs(
                    bodyTwist.OmegaRadiansPerSecond) >
                MotionDeadband;

            if (!hasLinearMotion && !hasAngularMotion)
            {
                return true;
            }

            return PreparationMode ==
                   FleetMemberPreparationMode.Rolling
                ? hasLinearMotion
                : !hasLinearMotion && hasAngularMotion;
        }

        /// <summary>拒绝未满足执行条件的命令并保持车辆零速。</summary>
        private bool RejectWhileStopped(string reason)
        {
            _adapter.StopImmediately();
            LastFailureReason = reason ?? string.Empty;
            return false;
        }

        private void AcceptCommandDeadline(
            double commandReceivedTimeSeconds,
            double validForSeconds)
        {
            var commandDeadlineSeconds =
                commandReceivedTimeSeconds +
                validForSeconds;
            NumericGuard.EnsureFinite(
                commandDeadlineSeconds,
                nameof(validForSeconds));

            _lastAcceptedCommandTimeSeconds =
                commandReceivedTimeSeconds;
            _commandDeadlineSeconds =
                commandDeadlineSeconds;
        }

        private void ClearCommandWatchdog()
        {
            _lastAcceptedCommandTimeSeconds = null;
            _commandDeadlineSeconds = null;
        }

        /// <summary>锁存成员车故障并立即清零驱动轮速度。</summary>
        private bool Fail(string reason)
        {
            _adapter.StopImmediately();
            State = FleetMemberAgentState.Faulted;
            LastFailureReason = reason ?? string.Empty;
            return false;
        }

        /// <summary>优先返回底盘提供的具体失败原因。</summary>
        private string BuildAdapterFailureReason(
            string fallbackReason)
        {
            return string.IsNullOrWhiteSpace(
                    _adapter.LastFailureReason)
                ? fallbackReason
                : _adapter.LastFailureReason;
        }

        /// <summary>拒绝零值和负值任务编号。</summary>
        private static void ValidatePlanId(long planId)
        {
            if (planId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(planId),
                    "车队动作任务编号必须大于零。");
            }
        }
    }
}