using System;
using System.Collections.Generic;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;

/// <summary>按公开字典序选择唯一的推荐平滑方法。</summary>
public static class SmoothingMethodRanker
{
    /// <summary>从当前场景的可行且确定性候选中选择最佳方法；没有合格候选时返回空。</summary>
    public static SmoothingMethod? Rank(IReadOnlyList<PathSmoothingComparisonEntry> entries)
    {
        PathSmoothingComparisonEntry best = null;
        if (entries == null) return null;

        for (int index = 0; index < entries.Count; index++)
        {
            PathSmoothingComparisonEntry candidate = entries[index];
            if (candidate == null || !candidate.IsEligibleForRecommendation) continue;
            if (best == null || Compare(candidate, best) < 0) best = candidate;
        }
        return best == null ? (SmoothingMethod?)null : best.Method;
    }

    private static int Compare(PathSmoothingComparisonEntry left, PathSmoothingComparisonEntry right)
    {
        int comparison = CompareAscending(left.Metrics.CurvatureVariationEnergy, right.Metrics.CurvatureVariationEnergy);
        if (comparison != 0) return comparison;

        comparison = CompareAscending(
            left.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter,
            right.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter);
        if (comparison != 0) return comparison;

        comparison = CompareDescending(left.Metrics.MinimumBodyClearanceMeters, right.Metrics.MinimumBodyClearanceMeters);
        if (comparison != 0) return comparison;

        comparison = CompareAscending(left.Metrics.LengthChangePercent, right.Metrics.LengthChangePercent);
        if (comparison != 0) return comparison;

        comparison = CompareAscending(left.Timing.MedianElapsedMilliseconds, right.Timing.MedianElapsedMilliseconds);
        if (comparison != 0) return comparison;

        return ((int)left.Method.Value).CompareTo((int)right.Method.Value);
    }

    private static int CompareAscending(double left, double right)
    {
        return Normalize(left).CompareTo(Normalize(right));
    }

    private static int CompareDescending(double left, double right)
    {
        return Normalize(right).CompareTo(Normalize(left));
    }

    private static double Normalize(double value)
    {
        return double.IsNaN(value) || double.IsInfinity(value) ? double.PositiveInfinity : value;
    }
}