using System;
using System.Collections.Generic;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public static class LongitudinalTerminalSchedule
{
    public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
        double minimumStabilizationDurationSeconds)
    {
        if (knotTimes == null || knotTimes.Count < 3 ||
            double.IsNaN(minimumStabilizationDurationSeconds) ||
            double.IsInfinity(minimumStabilizationDurationSeconds) ||
            minimumStabilizationDurationSeconds <= 0d)
        {
            throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
        }

        double previous = double.NegativeInfinity;
        for (int index = 0; index < knotTimes.Count; index++)
        {
            if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
                knotTimes[index] <= previous)
            {
                throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
            }
            previous = knotTimes[index];
        }

        double finalTime = knotTimes[knotTimes.Count - 1];
        for (int index = knotTimes.Count - 2; index >= 1; index--)
        {
            if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
                return index;
        }

        throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
    }
}