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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>按方向段局部弧长插值平滑算法的原始路径参考。</summary>
internal static class PathReferenceInterpolator
{
    internal static bool TryInterpolateByArcLength(
        IReadOnlyList<SmoothingPoint2D> points,
        double targetArcLength,
        out SmoothingPoint2D reference,
        out string reason)
    {
        reference = null;
        reason = string.Empty;
        if (points == null || points.Count == 0 || !NumericGuard.IsFinite(targetArcLength))
        {
            reason = "原始路径参考点或目标弧长无效。";
            return false;
        }

        for (int index = 0; index < points.Count; index++)
        {
            SmoothingPoint2D point = points[index];
            if (!IsValid(point) || (index > 0 && point.ArcLength < points[index - 1].ArcLength))
            {
                reason = "原始路径参考点包含非法数值或非递增弧长。";
                return false;
            }
        }

        SmoothingPoint2D first = points[0];
        SmoothingPoint2D last = points[points.Count - 1];
        if (targetArcLength < first.ArcLength || targetArcLength > last.ArcLength)
        {
            reason = "目标弧长不在原始方向段范围内。";
            return false;
        }

        if (targetArcLength == first.ArcLength)
        {
            reference = first;
            return true;
        }
        if (targetArcLength == last.ArcLength)
        {
            reference = last;
            return true;
        }

        for (int rightIndex = 1; rightIndex < points.Count; rightIndex++)
        {
            SmoothingPoint2D left = points[rightIndex - 1];
            SmoothingPoint2D right = points[rightIndex];
            if (targetArcLength > right.ArcLength) continue;
            if (targetArcLength == right.ArcLength)
            {
                reference = right;
                return true;
            }

            double interval = right.ArcLength - left.ArcLength;
            if (!NumericGuard.IsPositiveFinite(interval))
            {
                reason = "原始路径参考点包含无法插值的重复弧长。";
                return false;
            }

            double ratio = (targetArcLength - left.ArcLength) / interval;
            reference = new SmoothingPoint2D(
                left.X + ratio * (right.X - left.X),
                left.Y + ratio * (right.Y - left.Y),
                targetArcLength,
                left.Heading + ratio * (right.Heading - left.Heading),
                left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
                left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
                false,
                SmoothedPathPointSource.Interpolated);
            if (!IsValid(reference))
            {
                reference = null;
                reason = "原始路径参考插值产生非法数值。";
                return false;
            }
            return true;
        }

        reason = "原始路径参考无法定位目标弧长。";
        return false;
    }

    private static bool IsValid(SmoothingPoint2D point)
    {
        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
            NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
            NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
            point.ArcLength >= 0d && point.BodyClearance >= 0d;
    }
}