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

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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable ST optimization knots, separate from the trajectory publication cadence.</summary>
public sealed class LongitudinalKnotSchedule
{
    public LongitudinalKnotSchedule(IReadOnlyList<double> knotTimes, IReadOnlyList<double> referencePathS,
        IReadOnlyList<double> referenceSpeedMetersPerSecond, bool isAdaptive)
        : this(knotTimes, referencePathS, referenceSpeedMetersPerSecond, isAdaptive, -1)
    {
    }

    public LongitudinalKnotSchedule(IReadOnlyList<double> knotTimes, IReadOnlyList<double> referencePathS,
        IReadOnlyList<double> referenceSpeedMetersPerSecond, bool isAdaptive,
        int terminalHoldStartIndex)
    {
        KnotTimes = CopyTimes(knotTimes);
        ReferencePathS = CopyNondecreasing(referencePathS, KnotTimes.Count, nameof(referencePathS));
        ReferenceSpeedMetersPerSecond = CopyNonnegative(referenceSpeedMetersPerSecond, KnotTimes.Count,
            nameof(referenceSpeedMetersPerSecond));
        if (isAdaptive && ReferenceSpeedMetersPerSecond[ReferenceSpeedMetersPerSecond.Count - 1] != 0d)
            throw new ArgumentException("An adaptive full-segment schedule must end at exact zero speed.",
                nameof(referenceSpeedMetersPerSecond));

        IsAdaptive = isAdaptive;
        TotalDurationSeconds = KnotTimes[KnotTimes.Count - 1];
        if (terminalHoldStartIndex != -1 &&
            (!isAdaptive || terminalHoldStartIndex < 1 || terminalHoldStartIndex >= KnotTimes.Count))
        {
            throw new ArgumentOutOfRangeException(nameof(terminalHoldStartIndex));
        }
        TerminalHoldStartIndex = terminalHoldStartIndex;
    }

    public IReadOnlyList<double> KnotTimes { get; }
    public IReadOnlyList<double> ReferencePathS { get; }
    public IReadOnlyList<double> ReferenceSpeedMetersPerSecond { get; }
    public double TotalDurationSeconds { get; }
    public bool IsAdaptive { get; }
    public int TerminalHoldStartIndex { get; }

    internal static LongitudinalKnotSchedule CreateAdaptive(IReadOnlyList<double> knotTimes,
        IReadOnlyList<double> referencePathS, IReadOnlyList<double> referenceSpeedMetersPerSecond,
        int terminalHoldStartIndex)
    {
        return new LongitudinalKnotSchedule(knotTimes, referencePathS, referenceSpeedMetersPerSecond, true,
            terminalHoldStartIndex);
    }

    internal LongitudinalKnotSchedule Copy()
    {
        return new LongitudinalKnotSchedule(KnotTimes, ReferencePathS, ReferenceSpeedMetersPerSecond, IsAdaptive,
            TerminalHoldStartIndex);
    }

    public static LongitudinalKnotSchedule CreateRolling(double timeHorizonSeconds, double timeStepSeconds)
    {
        IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(timeHorizonSeconds, timeStepSeconds);
        var pathS = new double[times.Count];
        var speeds = new double[times.Count];
        return new LongitudinalKnotSchedule(times, pathS, speeds, false);
    }

    private static IReadOnlyList<double> CopyTimes(IReadOnlyList<double> source)
    {
        if (source == null || source.Count < 2)
            throw new ArgumentException("At least two time knots are required.", nameof(source));
        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 || (index == 0 && source[index] != 0d))
                throw new ArgumentException("Time knots must be finite, begin at exact zero, and strictly increase.",
                    nameof(source));
            copy.Add(source[index]);
            previous = source[index];
        }
        return new ReadOnlyCollection<double>(copy);
    }

    private static IReadOnlyList<double> CopyNondecreasing(IReadOnlyList<double> source, int expectedCount,
        string parameterName)
    {
        if (source == null || source.Count != expectedCount || source[0] != 0d)
            throw new ArgumentException("Reference PathS must begin at exact zero and match the knot count.", 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("Reference PathS must be finite and nondecreasing.", parameterName);
            copy.Add(source[index]);
            previous = source[index];
        }
        return new ReadOnlyCollection<double>(copy);
    }

    private static IReadOnlyList<double> CopyNonnegative(IReadOnlyList<double> source, int expectedCount,
        string parameterName)
    {
        if (source == null || source.Count != expectedCount)
            throw new ArgumentException("Reference speeds must match the knot count.", 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) => !double.IsNaN(value) && !double.IsInfinity(value);

}