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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 在指定方向段及局部 S 窗口内确定性地投影世界位姿。
/// 投影不跨越换向边界；世界 X/Y 与距离为 m，航向为 rad，并以种子 S 消除等距候选的拓扑歧义。
/// </summary>
public sealed class FrenetProjector
{
    /// <summary>
    /// 等距候选比较和零长度线段识别使用的 S/距离平方数值容差 1e-14。
    /// </summary>
    private const double TieTolerance = 1e-14d;

    /// <summary>
    /// 在窗口内投影世界位姿，并以窗口起点作为等距候选的种子 S。
    /// 参数：窗口与 maximumDistanceMeters 均为当前段局部 m 制距离；返回：命中距离不超过阈值时返回 true，否则 projection 为 null。
    /// </summary>
    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
        double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
    {
        return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters,
            minimumReferenceS, out projection);
    }

    /// <summary>
    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
    /// </summary>
    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
        double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
    {
        projection = null;
        if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) ||
            !IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) ||
            !IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d)
            return false;

        double lowerBound = Math.Max(0d, minimumReferenceS);
        double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS);
        if (lowerBound > upperBound)
            return false;

        Candidate best = null;
        for (int index = 0; index + 1 < segment.Points.Count; index++)
        {
            double startS = Math.Max(lowerBound, segment.Points[index].ArcLength);
            double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength);
            if (startS > endS)
                continue;

            FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS);
            FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS);
            ConsiderLine(worldPose, start, end, seedReferenceS, ref best);
        }

        if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance)
        {
            FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound);
            ConsiderPoint(worldPose, point, seedReferenceS, ref best);
        }

        if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters)
            return false;

        FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS);
        double dx = worldPose.X - reference.X;
        double dy = worldPose.Y - reference.Y;
        double travelYaw = reference.TravelYaw;
        double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw);
        double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction);
        double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw);
        projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters);
        return true;
    }

    /// <summary>
    /// 考察被 S 窗口裁剪后的参考折线边，并将世界点的正交投影加入最优候选比较。
    /// 参数：端点为世界 X/Y（m）和局部 S（m），seedReferenceS 为 m；零长度（不超过 1e-14）边退化为两个端点比较。
    /// </summary>
    private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
        double seedReferenceS, ref Candidate best)
    {
        double dx = end.X - start.X;
        double dy = end.Y - start.Y;
        double lengthSquared = dx * dx + dy * dy;
        if (lengthSquared <= TieTolerance)
        {
            ConsiderPoint(worldPose, start, seedReferenceS, ref best);
            ConsiderPoint(worldPose, end, seedReferenceS, ref best);
            return;
        }

        double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared;
        fraction = Math.Max(0d, Math.Min(1d, fraction));
        double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction;
        double x = start.X + dx * fraction;
        double y = start.Y + dy * fraction;
        Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
    }

    /// <summary>
    /// 将一个离散参考点作为投影候选参与比较。
    /// 参数：point 的位置为世界 m 制坐标，seedReferenceS 为局部 m 制种子；结果通过 best 原位更新。
    /// </summary>
    private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
        ref Candidate best)
    {
        Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
    }

    /// <summary>
    /// 依据世界平面平方距离登记候选，并按确定性优先级替换当前最佳值。
    /// 参数：referenceS、x、y 与 seedReferenceS 分别为局部 S（m）和世界坐标（m）；平方距离由内部计算，单位 m²。
    /// </summary>
    private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
        ref Candidate best)
    {
        double dx = worldPose.X - x;
        double dy = worldPose.Y - y;
        double squaredDistance = dx * dx + dy * dy;
        var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS));
        if (best == null || candidate.IsPreferredTo(best))
            best = candidate;
    }

    /// <summary>
    /// 判定投影计算的标量是否有限。
    /// 参数：value 为任意实数；返回：NaN 与无穷均返回 false。
    /// </summary>
    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }

    /// <summary>
    /// 保存一个待比较的折线投影候选。
    /// ReferenceS 和 SeedDistance 使用 m，SquaredDistanceMeters 使用 m²，优先级由距离、种子距离及较小 S 依次决定。
    /// </summary>
    private sealed class Candidate
    {
        /// <summary>
        /// 创建投影候选。
        /// 参数：referenceS 与 seedDistance 为 m，squaredDistanceMeters 为 m²；调用方仅传入有限的已计算值。
        /// </summary>
        public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
        {
            ReferenceS = referenceS;
            SquaredDistanceMeters = squaredDistanceMeters;
            SeedDistance = seedDistance;
        }

        /// <summary>
        /// 候选在当前方向段的局部参考弧长 S，单位 m。
        /// </summary>
        public double ReferenceS { get; }

        /// <summary>
        /// 候选世界位置到车辆位置的距离平方，单位 m²。
        /// </summary>
        public double SquaredDistanceMeters { get; }

        /// <summary>
        /// 候选 S 与调用方种子 S 的绝对距离，单位 m。
        /// </summary>
        public double SeedDistance { get; }

        /// <summary>
        /// 比较两个候选的稳定优先级。
        /// 参数：other 为非空候选；返回：平方距离差超过 1e-14 时取较小者，随后取较近种子，仍相等时取较小 S。
        /// </summary>
        public bool IsPreferredTo(Candidate other)
        {
            if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
            if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false;
            if (SeedDistance < other.SeedDistance - TieTolerance) return true;
            if (SeedDistance > other.SeedDistance + TieTolerance) return false;
            return ReferenceS < other.ReferenceS;
        }
    }
}
    /// <summary>
    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
    /// </summary>