using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Connected free lateral interval at one exact reference-S station.</summary>
public sealed class LateralInterval
{
    public LateralInterval(double referenceS, double minimumL, double maximumL, double seedL)
    {
        if (!IsFinite(referenceS) || !IsFinite(minimumL) || !IsFinite(maximumL) || !IsFinite(seedL) ||
            minimumL > maximumL || seedL < minimumL - 1e-12d || seedL > maximumL + 1e-12d)
            throw new ArgumentOutOfRangeException(nameof(referenceS));

        ReferenceS = referenceS;
        MinimumL = minimumL;
        MaximumL = maximumL;
        SeedL = seedL;
    }

    public double ReferenceS { get; }
    public double MinimumL { get; }
    public double MaximumL { get; }
    public double SeedL { get; }

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