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

using System;
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 terminalReferenceS, EmTerminalType terminalType)
    {
        TerminalReferenceS = terminalReferenceS;
        TerminalType = terminalType;
    }

    public double TerminalReferenceS { get; }

    public EmTerminalType TerminalType { 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);
        LongitudinalStoppingProfile initialStop = LongitudinalStoppingMath.Calculate(initialProgressSpeedMetersPerSecond,
            initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
            longitudinal.MaximumJerkMetersPerSecondCubed);
        if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
        {
            failureReason = "The current segment lacks the jerk-limited stopping distance.";
            return EmPlanningStatus.StoppingDistanceInsufficient;
        }

        double timeReachable = CalculateReachableDistance(initialProgressSpeedMetersPerSecond,
            initialAccelerationMetersPerSecondSquared, directionMaximum, longitudinal, scheduling.TimeHorizonSeconds);
        double terminalReferenceS = currentSegmentReferenceS + Math.Min(remainingSegment,
            Math.Min(scheduling.DistanceHorizonMeters, timeReachable));
        if (terminalReferenceS >= segment.LengthMeters - BoundaryTolerance)
        {
            terminalReferenceS = segment.LengthMeters;
            selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType));
        }
        else
        {
            selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop);
        }
        return EmPlanningStatus.Success;
    }

    private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
        LongitudinalConfiguration configuration, double timeHorizonSeconds)
    {
        LongitudinalStoppingProfile stopAtMaximumSpeed = LongitudinalStoppingMath.Calculate(maximumSpeed, 0d,
            configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
        double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds;
        if (drivingDuration <= 0d)
            return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds);

        double speed = initialSpeed;
        double acceleration = initialAcceleration;
        double distance = 0d;
        const double simulationStepSeconds = 0.001d;
        while (drivingDuration > 0d)
        {
            double step = Math.Min(simulationStepSeconds, drivingDuration);
            double jerk = ChooseAccelerationJerk(speed, acceleration, maximumSpeed,
                configuration.MaximumAccelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
            double nextSpeed = speed + acceleration * step + 0.5d * jerk * step * step;
            if (nextSpeed > maximumSpeed)
            {
                nextSpeed = maximumSpeed;
                acceleration = 0d;
                jerk = 0d;
            }
            distance += speed * step + 0.5d * acceleration * step * step + jerk * step * step * step / 6d;
            acceleration += jerk * step;
            speed = Math.Max(0d, nextSpeed);
            drivingDuration -= step;
        }
        return Math.Max(0d, distance + stopAtMaximumSpeed.DistanceMeters);
    }

    private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed,
        double maximumAcceleration, double maximumJerk)
    {
        if (speed >= maximumSpeed - BoundaryTolerance)
        {
            if (acceleration > 0d)
                return -maximumJerk;
            return acceleration < 0d ? maximumJerk : 0d;
        }
        double speedToReduceAccelerationToZero = acceleration > 0d
            ? acceleration * acceleration / (2d * maximumJerk)
            : 0d;
        if (speed + speedToReduceAccelerationToZero >= maximumSpeed - BoundaryTolerance)
            return acceleration > 0d ? -maximumJerk : 0d;
        return acceleration < maximumAcceleration - BoundaryTolerance ? maximumJerk : 0d;
    }

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