ParkingRobot/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs

using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Pure caller-clocked state machine for a zero-speed direction change.</summary>
public sealed class GearSwitchStateMachine
{
    private readonly double stopSpeedToleranceMetersPerSecond;
    private readonly TimeSpan zeroSpeedHoldDuration;
    private DateTimeOffset? zeroSpeedSince;

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

    public GearSwitchState State { get; private set; } = GearSwitchState.Following;

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

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

    public GearSwitchState State { get; }

    public bool HoldZero { get; }

    public bool RequestDirectionChange { get; }

    public bool AllowsTrajectoryMotion { get; }

    public bool IsTrajectoryComplete { get; }

    public string Reason { get; }
}