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

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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable time-knot s/u/a/j iterate used for ST linearization and strict validation.</summary>
public sealed class LongitudinalCandidate
{
    public LongitudinalCandidate(IReadOnlyList<double> knotTimes, IReadOnlyList<double> s, IReadOnlyList<double> u,
        IReadOnlyList<double> a, IReadOnlyList<double> j)
    {
        KnotTimes = CopyTimes(knotTimes);
        S = CopyValues(s, KnotTimes.Count, nameof(s));
        U = CopyValues(u, KnotTimes.Count, nameof(u));
        A = CopyValues(a, KnotTimes.Count, nameof(a));
        J = CopyValues(j, KnotTimes.Count - 1, nameof(j));
    }

    public IReadOnlyList<double> KnotTimes { get; }

    public IReadOnlyList<double> S { get; }

    public IReadOnlyList<double> U { get; }

    public IReadOnlyList<double> A { get; }

    public IReadOnlyList<double> J { get; }

    public static LongitudinalCandidate Integrate(IReadOnlyList<double> knotTimes, double initialS, double initialU,
        double initialA, IReadOnlyList<double> jerk)
    {
        IReadOnlyList<double> times = CopyTimes(knotTimes);
        if (!IsFinite(initialS) || !IsFinite(initialU) || !IsFinite(initialA))
            throw new ArgumentOutOfRangeException(nameof(initialS));
        IReadOnlyList<double> copiedJerk = CopyValues(jerk, times.Count - 1, nameof(jerk));
        var s = new double[times.Count];
        var u = new double[times.Count];
        var a = new double[times.Count];
        s[0] = initialS;
        u[0] = initialU;
        a[0] = initialA;
        for (int index = 0; index < copiedJerk.Count; index++)
        {
            double dt = times[index + 1] - times[index];
            double currentJerk = copiedJerk[index];
            a[index + 1] = a[index] + dt * currentJerk;
            u[index + 1] = u[index] + dt * a[index] + 0.5d * dt * dt * currentJerk;
            s[index + 1] = s[index] + dt * u[index] + 0.5d * dt * dt * a[index] +
                dt * dt * dt * currentJerk / 6d;
        }
        return new LongitudinalCandidate(times, s, u, a, copiedJerk);
    }

    public bool SatisfiesExactDiscreteDynamics(double tolerance)
    {
        if (!IsFinite(tolerance) || tolerance < 0d)
            throw new ArgumentOutOfRangeException(nameof(tolerance));
        for (int index = 0; index < J.Count; index++)
        {
            double dt = KnotTimes[index + 1] - KnotTimes[index];
            if (Math.Abs(A[index + 1] - (A[index] + dt * J[index])) > tolerance ||
                Math.Abs(U[index + 1] - (U[index] + dt * A[index] + 0.5d * dt * dt * J[index])) > tolerance ||
                Math.Abs(S[index + 1] - (S[index] + dt * U[index] + 0.5d * dt * dt * A[index] +
                    dt * dt * dt * J[index] / 6d)) > tolerance)
            {
                return false;
            }
        }
        return true;
    }

    public static IReadOnlyList<double> CreateKnotTimes(double timeHorizonSeconds, double outputTimeStepSeconds)
    {
        if (!IsFinite(timeHorizonSeconds) || !IsFinite(outputTimeStepSeconds) || timeHorizonSeconds <= 0d ||
            outputTimeStepSeconds <= 0d)
        {
            throw new ArgumentOutOfRangeException(nameof(timeHorizonSeconds));
        }
        int intervalCount = checked((int)Math.Ceiling(timeHorizonSeconds / outputTimeStepSeconds));
        var times = new double[intervalCount + 1];
        for (int index = 0; index < intervalCount; index++)
            times[index] = index * outputTimeStepSeconds;
        times[intervalCount] = timeHorizonSeconds;
        return new ReadOnlyCollection<double>(times);
    }

    private static IReadOnlyList<double> CopyTimes(IReadOnlyList<double> source)
    {
        if (source == null || source.Count < 2)
            throw new ArgumentException("At least two strictly increasing 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)
                throw new ArgumentException("Time knots must be finite and strictly increasing.", nameof(source));
            if (index == 0 && Math.Abs(source[index]) > 1e-12d)
                throw new ArgumentException("The first time knot must be exact zero.", nameof(source));
            copy.Add(source[index]);
            previous = source[index];
        }
        return new ReadOnlyCollection<double>(copy);
    }

    private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> source, int expectedCount, string parameterName)
    {
        if (source == null || source.Count != expectedCount)
            throw new ArgumentException("Longitudinal values do not match the time-knot layout.", parameterName);
        var copy = new List<double>(source.Count);
        for (int index = 0; index < source.Count; index++)
        {
            if (!IsFinite(source[index]))
                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);
    }
}