using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>World pose projected onto one bounded direction segment.</summary>
public sealed class FrenetProjection
{
    public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
        double squaredDistanceMeters)
    {
        if (referencePoint == null)
            throw new ArgumentNullException(nameof(referencePoint));
        if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
            throw new ArgumentOutOfRangeException(nameof(lateralOffset));

        ReferencePoint = referencePoint;
        ReferenceS = referencePoint.ReferenceS;
        LateralOffset = lateralOffset;
        HeadingError = headingError;
        SquaredDistanceMeters = squaredDistanceMeters;
    }

    public FrenetReferencePoint ReferencePoint { get; }
    public double ReferenceS { get; }
    public double LateralOffset { get; }
    public double HeadingError { get; }
    public double SquaredDistanceMeters { get; }

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