using System;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
public static class ReferencePathInterpolator
{
    private const double Epsilon = 1e-12d;

    public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
    {
        if (segment == null)
            throw new ArgumentNullException(nameof(segment));
        if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon)
            throw new ArgumentOutOfRangeException(nameof(referenceS));

        double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS));
        for (int index = 0; index < segment.Points.Count; index++)
        {
            SmoothedPathPoint upper = segment.Points[index];
            if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon)
                return FromPoint(upper);
            if (upper.ArcLength > clampedS)
            {
                SmoothedPathPoint lower = segment.Points[index - 1];
                double span = upper.ArcLength - lower.ArcLength;
                if (span <= Epsilon)
                    throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment));
                double fraction = (clampedS - lower.ArcLength) / span;
                return new FrenetReferencePoint(
                    clampedS,
                    Lerp(lower.X, upper.X, fraction),
                    Lerp(lower.Y, upper.Y, fraction),
                    AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)),
                    Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction),
                    segment.Direction,
                    Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
                    Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
                    Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
                    Lerp(lower.BodyClearance, upper.BodyClearance, fraction));
            }
        }
        return FromPoint(segment.Points[segment.Points.Count - 1]);
    }

    private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
    {
        return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
            point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
            point.BodyClearance);
    }

    private static double Lerp(double lower, double upper, double fraction)
    {
        return lower + (upper - lower) * fraction;
    }

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