using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Deterministic variable ranges for one lateral QP discretized on N stations.</summary>
public sealed class LateralVariableLayout
{
    public LateralVariableLayout(int stationCount)
    {
        if (stationCount < 2)
            throw new ArgumentOutOfRangeException(nameof(stationCount), "At least two reference-S stations are required.");

        StationCount = stationCount;
        LStart = 0;
        DLStart = stationCount;
        DDLStart = 2 * stationCount;
        DDDLStart = 3 * stationCount;
        VariableCount = 4 * stationCount - 1;
    }

    public int StationCount { get; }

    public int LStart { get; }

    public int DLStart { get; }

    public int DDLStart { get; }

    public int DDDLStart { get; }

    public int VariableCount { get; }

    public int L(int stationIndex)
    {
        RequireStationIndex(stationIndex);
        return LStart + stationIndex;
    }

    public int DL(int stationIndex)
    {
        RequireStationIndex(stationIndex);
        return DLStart + stationIndex;
    }

    public int DDL(int stationIndex)
    {
        RequireStationIndex(stationIndex);
        return DDLStart + stationIndex;
    }

    public int DDDL(int intervalIndex)
    {
        if (intervalIndex < 0 || intervalIndex >= StationCount - 1)
            throw new ArgumentOutOfRangeException(nameof(intervalIndex));
        return DDDLStart + intervalIndex;
    }

    private void RequireStationIndex(int stationIndex)
    {
        if (stationIndex < 0 || stationIndex >= StationCount)
            throw new ArgumentOutOfRangeException(nameof(stationIndex));
    }
}