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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 在恰好一个方向段内按局部参考弧长插值参考几何。
/// 输入和输出 S、X/Y 与净空均为 m，航向为 rad，曲率为 1/m；插值不会越过方向段边界。
/// </summary>
public static class ReferencePathInterpolator
{
    /// <summary>
    /// 接受端点轻微 S 超差并判断精确节点的数值容差，单位 m。
    /// </summary>
    private const double Epsilon = 1e-12d;

    /// <summary>
    /// 为给定局部 S 生成方向一致的 Frenet 参考样本。
    /// 参数：segment 不可为空，referenceS 为 m；[-1e-12, Length+1e-12] 内的端点超差会夹紧到边界。
    /// 返回：精确点直接转换，区间内对位置、展开航向和几何量线性插值；超界或退化跨度会引发异常。
    /// </summary>
    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]);
    }

    /// <summary>
    /// 将原始平滑路径点转换为同一局部 S 的 Frenet 参考样本。
    /// 参数：point 已携带世界 X/Y（m）、航向（rad）和曲率量；返回：保持其方向及所有几何量的不可变副本。
    /// </summary>
    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);
    }

    /// <summary>
    /// 在线性标量区间内计算插值值。
    /// 参数：lower、upper 为同单位端点，fraction 为无单位比例；返回：同单位的未夹紧线性结果。
    /// </summary>
    private static double Lerp(double lower, double upper, double fraction)
    {
        return lower + (upper - lower) * fraction;
    }

    /// <summary>
    /// 判定插值输入是否为有限实数。
    /// 参数：value 为任意标量；返回：NaN 或无穷时为 false。
    /// </summary>
    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}