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

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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable longitudinal inputs derived only from an independently validated lateral PathS path.</summary>
public sealed class LongitudinalPlanningInput
{
    private const double PathSTolerance = 1e-12d;

    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
    {
        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
            throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
                nameof(path));
        if (!Enum.IsDefined(typeof(TravelDirection), direction))
            throw new ArgumentOutOfRangeException(nameof(direction));
        if (!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d)
            throw new ArgumentOutOfRangeException(nameof(initialProgressSpeedMetersPerSecond));
        if (!IsFinite(initialAccelerationMetersPerSecondSquared))
            throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
            throw new ArgumentOutOfRangeException(nameof(terminalType));
        if (configuration == null)
            throw new ArgumentNullException(nameof(configuration));

        Path = CopyAndValidatePath(path);
        Direction = direction;
        InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
        InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
        TerminalType = terminalType;
        Configuration = configuration.Copy();
        PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
        PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
            nameof(previousProgressSpeedMetersPerSecond));
        if (PreviousPathS.Count != PreviousProgressSpeedMetersPerSecond.Count)
            throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.",
                nameof(previousProgressSpeedMetersPerSecond));
    }

    public LateralPath Path { get; }

    public TravelDirection Direction { get; }

    public double InitialProgressSpeedMetersPerSecond { get; }

    public double InitialAccelerationMetersPerSecondSquared { get; }

    public EmTerminalType TerminalType { get; }

    public EmPlannerConfiguration Configuration { get; }

    public IReadOnlyList<double> PreviousPathS { get; }

    public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }

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

    public double DirectionMaximumSpeedMetersPerSecond
    {
        get
        {
            return Direction == TravelDirection.Forward
                ? Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond
                : Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond;
        }
    }

    private static LateralPath CopyAndValidatePath(LateralPath source)
    {
        var copy = new List<LateralPathPoint>(source.Points.Count);
        double previousPathS = double.NegativeInfinity;
        for (int index = 0; index < source.Points.Count; index++)
        {
            LateralPathPoint point = source.Points[index];
            if (point == null || !IsFinite(point.PathS) || point.PathS <= previousPathS)
                throw new ArgumentException("Lateral PathS must be finite and strictly increasing for ST planning.", nameof(source));
            if (index == 0 && Math.Abs(point.PathS) > PathSTolerance)
                throw new ArgumentException("The lateral PathS supplied to ST must begin at zero.", nameof(source));
            copy.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
                point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
                point.VehicleCurvatureDerivative));
            previousPathS = point.PathS;
        }
        return new LateralPath(copy, true);
    }

    private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, string parameterName)
    {
        var copy = new List<double>(source == null ? 0 : source.Count);
        if (source != null)
        {
            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);
    }
}