using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable world-space sample reconstructed from one lateral candidate.</summary>
public sealed class LateralPathPoint
{
    public LateralPathPoint(double referenceS, double pathS, double l, double dl, double ddl, double dddl,
        double x, double y, double vehicleYaw, double geometricCurvature, double vehicleCurvature,
        double vehicleCurvatureDerivative)
    {
        RequireFinite(referenceS, nameof(referenceS));
        RequireFinite(pathS, nameof(pathS));
        RequireFinite(l, nameof(l));
        RequireFinite(dl, nameof(dl));
        RequireFinite(ddl, nameof(ddl));
        RequireFinite(dddl, nameof(dddl));
        RequireFinite(x, nameof(x));
        RequireFinite(y, nameof(y));
        RequireFinite(vehicleYaw, nameof(vehicleYaw));
        RequireFinite(geometricCurvature, nameof(geometricCurvature));
        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
        if (referenceS < 0d)
            throw new ArgumentOutOfRangeException(nameof(referenceS));
        if (pathS < 0d)
            throw new ArgumentOutOfRangeException(nameof(pathS));

        ReferenceS = referenceS;
        PathS = pathS;
        L = l;
        DL = dl;
        DDL = ddl;
        DDDL = dddl;
        X = x;
        Y = y;
        VehicleYaw = vehicleYaw;
        GeometricCurvature = geometricCurvature;
        VehicleCurvature = vehicleCurvature;
        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
    }

    public double ReferenceS { get; }
    public double PathS { get; }
    public double L { get; }
    public double DL { get; }
    public double DDL { get; }
    public double DDDL { get; }
    public double X { get; }
    public double Y { get; }
    public double VehicleYaw { get; }
    public double GeometricCurvature { get; }
    public double VehicleCurvature { get; }
    public double VehicleCurvatureDerivative { get; }

    private static void RequireFinite(double value, string parameterName)
    {
        if (double.IsNaN(value) || double.IsInfinity(value))
            throw new ArgumentOutOfRangeException(parameterName);
    }
}