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

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Finite piecewise-linear speed limits indexed exclusively by actual lateral PathS.</summary>
public sealed class PathSpeedLimit
{
    private const double StationTolerance = 1e-12d;

    internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
        IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
        IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
    {
        PathS = CopyStrictStations(pathS, nameof(pathS));
        MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
        LateralAccelerationSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(lateralAccelerationLimit, PathS.Count,
            nameof(lateralAccelerationLimit));
        CurvatureRateSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(curvatureRateLimit, PathS.Count,
            nameof(curvatureRateLimit));
        StoppingSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(stoppingLimit, PathS.Count, nameof(stoppingLimit));
        if (!IsFinite(directionMaximumSpeedMetersPerSecond) || directionMaximumSpeedMetersPerSecond <= 0d)
            throw new ArgumentOutOfRangeException(nameof(directionMaximumSpeedMetersPerSecond));
        if (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
            StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d)
        {
            throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
        }

        DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
    }

    public IReadOnlyList<double> PathS { get; }

    public IReadOnlyList<double> MaximumSpeedMetersPerSecond { get; }

    public IReadOnlyList<double> LateralAccelerationSpeedLimitsMetersPerSecond { get; }

    public IReadOnlyList<double> CurvatureRateSpeedLimitsMetersPerSecond { get; }

    public IReadOnlyList<double> StoppingSpeedLimitsMetersPerSecond { get; }

    public double DirectionMaximumSpeedMetersPerSecond { get; }

    public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }

    public double MaximumSpeedAt(double pathS)
    {
        return Interpolate(MaximumSpeedMetersPerSecond, pathS);
    }

    public double LateralAccelerationLimitAt(double pathS)
    {
        return Interpolate(LateralAccelerationSpeedLimitsMetersPerSecond, pathS);
    }

    public double CurvatureRateLimitAt(double pathS)
    {
        return Interpolate(CurvatureRateSpeedLimitsMetersPerSecond, pathS);
    }

    public double StoppingLimitAt(double pathS)
    {
        return Interpolate(StoppingSpeedLimitsMetersPerSecond, pathS);
    }

    private double Interpolate(IReadOnlyList<double> values, double pathS)
    {
        if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
            throw new ArgumentOutOfRangeException(nameof(pathS));
        if (pathS <= PathS[0])
            return values[0];
        if (pathS >= TerminalPathS)
            return values[values.Count - 1];

        for (int index = 1; index < PathS.Count; index++)
        {
            if (pathS <= PathS[index])
            {
                double fraction = (pathS - PathS[index - 1]) / (PathS[index] - PathS[index - 1]);
                return values[index - 1] + (values[index] - values[index - 1]) * fraction;
            }
        }
        return values[values.Count - 1];
    }

    private static IReadOnlyList<double> CopyStrictStations(IReadOnlyList<double> source, string parameterName)
    {
        if (source == null || source.Count < 2)
            throw new ArgumentException("At least two PathS stations are required.", parameterName);
        var copy = new List<double>(source.Count);
        double previous = double.NegativeInfinity;
        for (int index = 0; index < source.Count; index++)
        {
            if (!IsFinite(source[index]) || source[index] <= previous)
                throw new ArgumentException("PathS stations must be finite and strictly increasing.", parameterName);
            copy.Add(source[index]);
            previous = source[index];
        }
        return new ReadOnlyCollection<double>(copy);
    }

    private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, int expectedCount,
        string parameterName)
    {
        if (source == null || source.Count != expectedCount)
            throw new ArgumentException("Speed limit count must match PathS stations.", parameterName);
        var copy = new List<double>(source.Count);
        for (int index = 0; index < source.Count; index++)
        {
            if (!IsFinite(source[index]) || source[index] < 0d)
                throw new ArgumentOutOfRangeException(parameterName);
            copy.Add(source[index]);
        }
        return new ReadOnlyCollection<double>(copy);
    }

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