using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Deterministic contiguous variable ranges for one time-domain longitudinal QP.</summary>
public sealed class LongitudinalVariableLayout
{
    public LongitudinalVariableLayout(int knotCount)
    {
        if (knotCount < 2)
            throw new ArgumentOutOfRangeException(nameof(knotCount), "At least two time knots are required.");

        KnotCount = knotCount;
        SStart = 0;
        UStart = knotCount;
        AStart = 2 * knotCount;
        JStart = 3 * knotCount;
        VariableCount = 4 * knotCount - 1;
    }

    public int KnotCount { get; }

    public int SStart { get; }

    public int UStart { get; }

    public int AStart { get; }

    public int JStart { get; }

    public int VariableCount { get; }

    public int S(int knotIndex) { return RequireKnotIndex(knotIndex, SStart); }

    public int U(int knotIndex) { return RequireKnotIndex(knotIndex, UStart); }

    public int A(int knotIndex) { return RequireKnotIndex(knotIndex, AStart); }

    public int J(int intervalIndex)
    {
        if (intervalIndex < 0 || intervalIndex >= KnotCount - 1)
            throw new ArgumentOutOfRangeException(nameof(intervalIndex));
        return JStart + intervalIndex;
    }

    private int RequireKnotIndex(int knotIndex, int start)
    {
        if (knotIndex < 0 || knotIndex >= KnotCount)
            throw new ArgumentOutOfRangeException(nameof(knotIndex));
        return start + knotIndex;
    }
}