ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
public sealed class PlanningHorizonSelection
{
    internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
        EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
        double stopBoundaryReferenceS, bool hasStopBoundary)
    {
        WindowEndReferenceS = windowEndReferenceS;
        WindowEndBoundaryType = windowEndBoundaryType;
        TerminalType = terminalType;
        LongitudinalMode = longitudinalMode;
        StopBoundaryReferenceS = stopBoundaryReferenceS;
        HasStopBoundary = hasStopBoundary;
    }

    public double WindowEndReferenceS { get; }

    public double TerminalReferenceS => WindowEndReferenceS;

    public EmBoundaryType WindowEndBoundaryType { get; }

    public EmTerminalType TerminalType { get; }

    public EmLongitudinalMode LongitudinalMode { get; }

    public double StopBoundaryReferenceS { get; }

    public bool HasStopBoundary { get; }
}

public sealed class PlanningHorizonSelector
{
    private const double BoundaryTolerance = 1e-8d;

    public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
        EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason)
    {
        selection = null;
        failureReason = string.Empty;
        if (segment == null || configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
            !IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
            currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
            !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
            !IsFinite(initialAccelerationMetersPerSecondSquared))
        {
            failureReason = "Planning horizon inputs are invalid.";
            return EmPlanningStatus.InvalidInput;
        }

        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
        SchedulingConfiguration scheduling = configuration.Scheduling;
        double directionMaximum = segment.Direction == TravelDirection.Forward
            ? longitudinal.MaximumForwardSpeedMetersPerSecond
            : longitudinal.MaximumReverseSpeedMetersPerSecond;
        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
            !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
            !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed) ||
            !IsPositiveFinite(longitudinal.ZeroSpeedHoldSeconds) || !IsPositiveFinite(scheduling.TimeHorizonSeconds) ||
            !IsPositiveFinite(scheduling.DistanceHorizonMeters))
        {
            failureReason = "Planning horizon configuration is invalid.";
            return EmPlanningStatus.InvalidInput;
        }
        if (initialProgressSpeedMetersPerSecond > directionMaximum + BoundaryTolerance ||
            initialAccelerationMetersPerSecondSquared < -longitudinal.MaximumDecelerationMetersPerSecondSquared - BoundaryTolerance ||
            initialAccelerationMetersPerSecondSquared > longitudinal.MaximumAccelerationMetersPerSecondSquared + BoundaryTolerance)
        {
            failureReason = "The initial state violates longitudinal bounds.";
            return EmPlanningStatus.InvalidInput;
        }

        double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
        if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
                initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile initialStop,
                out failureReason))
        {
            return EmPlanningStatus.InvalidInput;
        }
        if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
        {
            failureReason = "The current segment lacks the jerk-limited stopping distance.";
            return EmPlanningStatus.StoppingDistanceInsufficient;
        }

        double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
        bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
        bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);
        EmBoundaryType windowEndBoundaryType = windowReachesSegmentEnd
            ? segment.EndBoundary.BoundaryType
            : EmBoundaryType.RollingSafetyStop;
        if (!hasStopBoundary)
        {
            selection = new PlanningHorizonSelection(windowEnd, windowEndBoundaryType,
                EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation,
                segment.LengthMeters, false);
            return EmPlanningStatus.Success;
        }

        IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
            scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
            knotTimes, scheduling.OutputTimeStepSeconds);
        double availableMotionTime = knotTimes[stabilizationStart];
        double maximumStoppedDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
            initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, directionMaximum,
            longitudinal.MaximumAccelerationMetersPerSecondSquared,
            longitudinal.MaximumDecelerationMetersPerSecondSquared,
            longitudinal.MaximumJerkMetersPerSecondCubed, availableMotionTime);
        EmLongitudinalMode mode = remainingSegment <= maximumStoppedDistance + BoundaryTolerance
            ? EmLongitudinalMode.ExactStopAtBoundary
            : EmLongitudinalMode.ApproachStopBoundary;
        selection = new PlanningHorizonSelection(segment.LengthMeters, windowEndBoundaryType,
            ToTerminalType(segment.EndBoundary.BoundaryType), mode, segment.LengthMeters, true);
        return EmPlanningStatus.Success;
    }

    private static bool IsStopBoundary(EmBoundaryType boundaryType)
    {
        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
    }

    private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
    {
        return boundaryType == EmBoundaryType.Goal
            ? EmTerminalType.Goal
            : boundaryType == EmBoundaryType.GearSwitchApproach || boundaryType == EmBoundaryType.GearSwitchDeparture
                ? EmTerminalType.GearSwitch
                : EmTerminalType.RollingSafetyStop;
    }

    private static bool IsPositiveFinite(double value)
    {
        return IsFinite(value) && value > 0d;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}