ParkingRobot/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs

using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>纯 caller-clocked 的零速换向状态机；只依据调用方时间、测量速度和方向确认推进状态。</summary>
public sealed class GearSwitchStateMachine
{
    private readonly double stopSpeedToleranceMetersPerSecond;
    private readonly TimeSpan zeroSpeedHoldDuration;
    private DateTimeOffset? zeroSpeedSince;

    /// <summary>创建换向状态机。</summary>
    /// <param name="stopSpeedToleranceMetersPerSecond">判定已停稳的绝对带符号速度上限，单位 m/s。</param>
    /// <param name="zeroSpeedHoldSeconds">发送方向请求前必须连续保持停稳的时长，单位 s。</param>
    public GearSwitchStateMachine(double stopSpeedToleranceMetersPerSecond, double zeroSpeedHoldSeconds)
    {
        if (!IsNonNegativeFinite(stopSpeedToleranceMetersPerSecond))
            throw new ArgumentOutOfRangeException(nameof(stopSpeedToleranceMetersPerSecond));
        if (!IsNonNegativeFinite(zeroSpeedHoldSeconds))
            throw new ArgumentOutOfRangeException(nameof(zeroSpeedHoldSeconds));

        this.stopSpeedToleranceMetersPerSecond = stopSpeedToleranceMetersPerSecond;
        zeroSpeedHoldDuration = TimeSpan.FromSeconds(zeroSpeedHoldSeconds);
    }

    /// <summary>当前不可回退的换向状态；初始为 <see cref="GearSwitchState.Following"/>。</summary>
    public GearSwitchState State { get; private set; } = GearSwitchState.Following;

    /// <summary>基于调用方时刻、测量速度和边界语义推进一次换向状态。</summary>
    /// <param name="now">调用方当前时刻，用于计算零速 dwell，不读取系统时钟。</param>
    /// <param name="measuredSignedSpeed">测量带符号纵向速度，单位 m/s。</param>
    /// <param name="desiredDirection">轨迹要求的实际行驶方向。</param>
    /// <param name="currentDirection">调用方确认的当前实际方向。</param>
    /// <param name="directionConfirmed">调用方是否已确认硬件方向切换完成。</param>
    /// <param name="atGearSwitchBoundary">选中点是否为精确换向接近边界。</param>
    /// <param name="atTerminal">选中点是否为 Goal 或 RollingSafetyStop 终端。</param>
    /// <returns>包含停零、一次换向请求、是否允许轨迹运动和完成语义的不可变状态更新。</returns>
    public GearSwitchStateUpdate Update(DateTimeOffset now, double measuredSignedSpeed, TravelDirection desiredDirection,
        TravelDirection currentDirection, bool directionConfirmed, bool atGearSwitchBoundary, bool atTerminal)
    {
        if (double.IsNaN(measuredSignedSpeed) || double.IsInfinity(measuredSignedSpeed))
            throw new ArgumentOutOfRangeException(nameof(measuredSignedSpeed));
        if (!Enum.IsDefined(typeof(TravelDirection), desiredDirection) ||
            !Enum.IsDefined(typeof(TravelDirection), currentDirection))
        {
            throw new ArgumentOutOfRangeException(nameof(desiredDirection));
        }

        if (atTerminal)
        {
            State = GearSwitchState.Completed;
            zeroSpeedSince = null;
            return Held("terminal boundary reached", false, true);
        }

        switch (State)
        {
            case GearSwitchState.Following:
                if (desiredDirection != currentDirection)
                {
                    State = GearSwitchState.ApproachingGearSwitch;
                    return Moving("approaching gear-switch boundary");
                }
                return Moving("following current direction segment");

            case GearSwitchState.ApproachingGearSwitch:
                if (!atGearSwitchBoundary)
                    return Moving("approaching gear-switch boundary");
                State = GearSwitchState.HoldingZero;
                zeroSpeedSince = IsStopped(measuredSignedSpeed) ? now : null;
                return Held("holding at gear-switch boundary", false, false);

            case GearSwitchState.HoldingZero:
                if (!IsStopped(measuredSignedSpeed))
                {
                    zeroSpeedSince = null;
                    return Held("measured speed remains above stop tolerance", false, false);
                }
                if (!zeroSpeedSince.HasValue)
                {
                    zeroSpeedSince = now;
                    return Held("zero-speed dwell started", false, false);
                }
                if (now - zeroSpeedSince.Value < zeroSpeedHoldDuration)
                    return Held("zero-speed dwell in progress", false, false);

                State = GearSwitchState.RequestingDirectionChange;
                return Held("requesting direction change", true, false);

            case GearSwitchState.RequestingDirectionChange:
                State = GearSwitchState.AwaitingDirectionConfirmation;
                return Held("awaiting direction confirmation", false, false);

            case GearSwitchState.AwaitingDirectionConfirmation:
                if (directionConfirmed && desiredDirection == currentDirection)
                {
                    State = GearSwitchState.Following;
                    zeroSpeedSince = null;
                    return Moving("direction change confirmed");
                }
                return Held("awaiting direction confirmation", false, false);

            case GearSwitchState.Completed:
                return Held("trajectory already completed", false, true);

            default:
                throw new InvalidOperationException("Unsupported gear-switch state.");
        }
    }

    private GearSwitchStateUpdate Moving(string reason)
    {
        return new GearSwitchStateUpdate(State, false, false, true, false, reason);
    }

    private GearSwitchStateUpdate Held(string reason, bool requestDirectionChange, bool isTrajectoryComplete)
    {
        return new GearSwitchStateUpdate(State, true, requestDirectionChange, false, isTrajectoryComplete, reason);
    }

    private bool IsStopped(double measuredSignedSpeed)
    {
        return Math.Abs(measuredSignedSpeed) < stopSpeedToleranceMetersPerSecond;
    }

    private static bool IsNonNegativeFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d;
    }
}

/// <summary>一次换向状态机更新的不可变安全决策，供执行器映射为零速度/制动或轨迹跟随命令。</summary>
public sealed class GearSwitchStateUpdate
{
    internal GearSwitchStateUpdate(GearSwitchState state, bool holdZero, bool requestDirectionChange,
        bool allowsTrajectoryMotion, bool isTrajectoryComplete, string reason)
    {
        State = state;
        HoldZero = holdZero;
        RequestDirectionChange = requestDirectionChange;
        AllowsTrajectoryMotion = allowsTrajectoryMotion;
        IsTrajectoryComplete = isTrajectoryComplete;
        Reason = reason ?? string.Empty;
    }

    /// <summary>更新后的换向状态。</summary>
    public GearSwitchState State { get; }

    public bool HoldZero { get; }

    /// <summary>是否仅在本次更新请求一次方向变更。</summary>
    public bool RequestDirectionChange { get; }

    public bool AllowsTrajectoryMotion { get; }

    /// <summary>是否因 Goal 或 RollingSafetyStop 完成轨迹并需要持续制动。</summary>
    public bool IsTrajectoryComplete { get; }

    public string Reason { get; }
}