ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs

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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal sealed class TrajectorySampleSchedule
{
    private const double ZeroTolerance = 1e-12d;

    public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
        EmLongitudinalMode mode)
    {
        if (candidate == null)
            throw new ArgumentNullException(nameof(candidate));
        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d)
            throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
        if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
            throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
            throw new ArgumentOutOfRangeException(nameof(mode));

        var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
        double previousPathS = double.NegativeInfinity;
        for (int index = 0; index < candidate.KnotTimes.Count; index++)
        {
            if (candidate.S[index] < previousPathS)
                throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
            if (candidate.U[index] < -ZeroTolerance)
                throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));

            samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
                candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
            previousPathS = candidate.S[index];
        }

        if (mode != EmLongitudinalMode.ExactStopAtBoundary)
        {
            Samples = new ReadOnlyCollection<TrajectorySample>(samples);
            TerminalAnchorSampleIndex = -1;
            return;
        }

        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
            outputTimeStepSeconds);
        double stopPathS = candidate.S[stabilizationStart];
        for (int index = stabilizationStart; index < candidate.S.Count; index++)
        {
            if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
                Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
                (index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
            {
                throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate));
            }
        }

        TerminalAnchorSampleIndex = stabilizationStart;
        TrajectorySample terminal = samples[samples.Count - 1];
        double holdElapsed = 0d;
        while (holdElapsed < holdDurationSeconds - ZeroTolerance)
        {
            holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds);
            samples.Add(new TrajectorySample(terminal.TimeFromStart + holdElapsed, terminal.PathS, 0d, 0d, 0d, true));
        }

        Samples = new ReadOnlyCollection<TrajectorySample>(samples);
    }

    public IReadOnlyList<TrajectorySample> Samples { get; }
    public int TerminalAnchorSampleIndex { get; }

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

internal sealed class TrajectorySample
{
    public TrajectorySample(double timeFromStart, double pathS, double progressSpeed, double acceleration, double jerk,
        bool isHoldSample)
    {
        TimeFromStart = timeFromStart;
        PathS = pathS;
        ProgressSpeed = progressSpeed;
        Acceleration = acceleration;
        Jerk = jerk;
        IsHoldSample = isHoldSample;
    }

    public double TimeFromStart { get; }
    public double PathS { get; }
    public double ProgressSpeed { get; }
    public double Acceleration { get; }
    public double Jerk { get; }
    public bool IsHoldSample { get; }
}