ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs

using System;
using System.Collections.Generic;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Builds curvature-aware, stopping-aware speed limits over actual optimized PathS.</summary>
public sealed class PathSpeedLimitBuilder
{
    internal const double CurvatureEpsilon = 1e-10d;
    private const double StopDistanceToleranceMeters = 1e-8d;

    public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
    {
        speedLimit = null;
        failureReason = string.Empty;
        if (input == null)
        {
            failureReason = "Longitudinal planning input is required.";
            return EmPlanningStatus.InvalidInput;
        }

        if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
                out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
                out failureReason))
        {
            return EmPlanningStatus.InvalidInput;
        }
        if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
            input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
            input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
        {
            failureReason = "The initial longitudinal state violates the configured hard bounds.";
            return EmPlanningStatus.InvalidInput;
        }

        LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond,
            input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk);
        if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
        {
            failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
            return EmPlanningStatus.StoppingDistanceInsufficient;
        }

        int count = input.Path.Points.Count;
        var pathS = new double[count];
        var maximum = new double[count];
        var lateral = new double[count];
        var curvatureRate = new double[count];
        var stopping = new double[count];
        for (int index = 0; index < count; index++)
        {
            LateralPathPoint point = input.Path.Points[index];
            pathS[index] = point.PathS;
            double lateralLimit = Math.Sqrt(maximumLateralAcceleration /
                Math.Max(Math.Abs(point.VehicleCurvature), CurvatureEpsilon));
            double curvatureRateLimit = maximumCurvatureRate /
                Math.Max(Math.Abs(point.VehicleCurvatureDerivative), CurvatureEpsilon);
            double remainingDistance = Math.Max(0d, input.TerminalPathS - point.PathS);
            double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * remainingDistance);
            lateral[index] = ClampFinite(lateralLimit, directionMaximum);
            curvatureRate[index] = ClampFinite(curvatureRateLimit, directionMaximum);
            stopping[index] = index == count - 1 ? 0d : ClampFinite(stoppingLimit, directionMaximum);
            maximum[index] = index == count - 1 ? 0d : Math.Min(directionMaximum,
                Math.Min(lateral[index], Math.Min(curvatureRate[index], stopping[index])));
        }

        try
        {
            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
            return EmPlanningStatus.Success;
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return EmPlanningStatus.InvalidInput;
        }
    }

    internal static bool TryGetLimits(LongitudinalPlanningInput input, out double directionMaximum,
        out double maximumAcceleration, out double maximumDeceleration, out double maximumJerk,
        out double maximumLateralAcceleration, out double maximumCurvatureRate, out string failureReason)
    {
        directionMaximum = 0d;
        maximumAcceleration = 0d;
        maximumDeceleration = 0d;
        maximumJerk = 0d;
        maximumLateralAcceleration = 0d;
        maximumCurvatureRate = 0d;
        failureReason = string.Empty;
        if (input.Configuration == null || input.Configuration.Longitudinal == null)
        {
            failureReason = "Longitudinal configuration is required.";
            return false;
        }

        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        directionMaximum = input.DirectionMaximumSpeedMetersPerSecond;
        maximumAcceleration = configuration.MaximumAccelerationMetersPerSecondSquared;
        maximumDeceleration = configuration.MaximumDecelerationMetersPerSecondSquared;
        maximumJerk = configuration.MaximumJerkMetersPerSecondCubed;
        maximumLateralAcceleration = configuration.MaximumLateralAccelerationMetersPerSecondSquared;
        maximumCurvatureRate = configuration.MaximumCurvatureRatePerMeterPerSecond;
        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
            !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
        {
            failureReason = "Longitudinal limits must be positive and finite.";
            return false;
        }
        return true;
    }

    private static double ClampFinite(double value, double maximum)
    {
        if (!IsFinite(value) || value < 0d)
            throw new ArgumentOutOfRangeException(nameof(value));
        return Math.Min(maximum, value);
    }

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

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

internal sealed class LongitudinalStoppingProfile
{
    public LongitudinalStoppingProfile(double distanceMeters, double durationSeconds)
    {
        DistanceMeters = distanceMeters;
        DurationSeconds = durationSeconds;
    }

    public double DistanceMeters { get; }

    public double DurationSeconds { get; }
}

internal static class LongitudinalStoppingMath
{
    public static LongitudinalStoppingProfile Calculate(double speedMetersPerSecond, double accelerationMetersPerSecondSquared,
        double maximumDecelerationMetersPerSecondSquared, double maximumJerkMetersPerSecondCubed)
    {
        if (!IsFinite(speedMetersPerSecond) || !IsFinite(accelerationMetersPerSecondSquared) ||
            !IsPositiveFinite(maximumDecelerationMetersPerSecondSquared) || !IsPositiveFinite(maximumJerkMetersPerSecondCubed))
        {
            throw new ArgumentOutOfRangeException(nameof(speedMetersPerSecond));
        }
        if (speedMetersPerSecond <= 0d)
            return new LongitudinalStoppingProfile(0d, 0d);

        double acceleration = Math.Max(-maximumDecelerationMetersPerSecondSquared, accelerationMetersPerSecondSquared);
        double rampDuration = (acceleration + maximumDecelerationMetersPerSecondSquared) / maximumJerkMetersPerSecondCubed;
        double speedAfterRamp = speedMetersPerSecond + acceleration * rampDuration -
            0.5d * maximumJerkMetersPerSecondCubed * rampDuration * rampDuration;
        if (speedAfterRamp <= 0d)
        {
            double root = (acceleration + Math.Sqrt(acceleration * acceleration + 2d * maximumJerkMetersPerSecondCubed *
                speedMetersPerSecond)) / maximumJerkMetersPerSecondCubed;
            double distance = speedMetersPerSecond * root + 0.5d * acceleration * root * root -
                maximumJerkMetersPerSecondCubed * root * root * root / 6d;
            return new LongitudinalStoppingProfile(Math.Max(0d, distance), root);
        }

        double rampDistance = speedMetersPerSecond * rampDuration + 0.5d * acceleration * rampDuration * rampDuration -
            maximumJerkMetersPerSecondCubed * rampDuration * rampDuration * rampDuration / 6d;
        double constantDecelerationDuration = speedAfterRamp / maximumDecelerationMetersPerSecondSquared;
        double constantDecelerationDistance = speedAfterRamp * speedAfterRamp /
            (2d * maximumDecelerationMetersPerSecondSquared);
        return new LongitudinalStoppingProfile(rampDistance + constantDecelerationDistance,
            rampDuration + constantDecelerationDuration);
    }

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

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