using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
public sealed class FrenetReferencePoint
{
    public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
        TravelDirection direction, double geometricCurvature, double vehicleCurvature,
        double vehicleCurvatureDerivative, double bodyClearance)
    {
        RequireFinite(referenceS, nameof(referenceS));
        RequireFinite(x, nameof(x));
        RequireFinite(y, nameof(y));
        RequireFinite(vehicleYaw, nameof(vehicleYaw));
        RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
        RequireFinite(geometricCurvature, nameof(geometricCurvature));
        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
        RequireFinite(bodyClearance, nameof(bodyClearance));

        ReferenceS = referenceS;
        X = x;
        Y = y;
        VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
        UnwrappedVehicleYaw = unwrappedVehicleYaw;
        Direction = direction;
        GeometricCurvature = geometricCurvature;
        VehicleCurvature = vehicleCurvature;
        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
        BodyClearance = bodyClearance;
    }

    public double ReferenceS { get; }
    public double X { get; }
    public double Y { get; }
    public double VehicleYaw { get; }
    public double UnwrappedVehicleYaw { get; }
    public TravelDirection Direction { get; }
    public double GeometricCurvature { get; }
    public double VehicleCurvature { get; }
    public double VehicleCurvatureDerivative { get; }
    public double BodyClearance { get; }

    /// <summary>Unwrapped direction of travel, used internally for geometry.</summary>
    public double TravelYaw
    {
        get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
    }

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