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

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;

/// <summary>
/// 为 Hybrid A* Open List 提供确定性优先级的二叉最小堆。
/// 排序严格依次比较 F、H、较大的 G 和插入序号；F、H、G 必须为有限且非负的等效米代价。
/// </summary>
/// <typeparam name="T">与一组搜索代价关联的节点或条目类型。</typeparam>
public sealed class BinaryMinHeap<T>
{
    private readonly List<HeapEntry> _entries = new List<HeapEntry>();
    private long _nextInsertionSequence;

    /// <summary>创建空的确定性 Open List 堆。</summary>
    public BinaryMinHeap()
    {
    }

    /// <summary>当前堆内尚未出队的条目数量。</summary>
    public int Count { get { return _entries.Count; } }

    /// <summary>
    /// 将一个条目和其搜索排序代价压入堆。
    /// 参数：item 为关联条目；f、h、g 均为有限且非负的等效米代价。
    /// 失败：任一代价无效、item 为 null（仅引用类型）或插入序号耗尽时抛出异常。
    /// </summary>
    public void Push(T item, double f, double h, double g)
    {
        if (ReferenceEquals(item, null)) throw new ArgumentNullException(nameof(item));
        ValidateCost(f, nameof(f));
        ValidateCost(h, nameof(h));
        ValidateCost(g, nameof(g));
        if (_nextInsertionSequence == long.MaxValue)
            throw new InvalidOperationException("The binary heap insertion sequence has been exhausted.");

        var entry = new HeapEntry(item, f, h, g, _nextInsertionSequence);
        _nextInsertionSequence++;
        _entries.Add(entry);
        SiftUp(_entries.Count - 1);
    }

    /// <summary>
    /// 弹出当前排序最优的条目。
    /// 返回：按 F、H、较大 G 和插入序号排序后的最小条目；空堆时抛出 <see cref="InvalidOperationException"/>。
    /// </summary>
    public T Pop()
    {
        if (_entries.Count == 0) throw new InvalidOperationException("The binary heap is empty.");

        HeapEntry result = _entries[0];
        int lastIndex = _entries.Count - 1;
        if (lastIndex == 0)
        {
            _entries.RemoveAt(0);
            return result.Item;
        }

        _entries[0] = _entries[lastIndex];
        _entries.RemoveAt(lastIndex);
        SiftDown(0);
        return result.Item;
    }

    /// <summary>清空尚未出队的条目；后续插入序号继续单调递增以保持整个实例内的确定性。</summary>
    public void Clear()
    {
        _entries.Clear();
    }

    private void SiftUp(int index)
    {
        while (index > 0)
        {
            int parentIndex = (index - 1) / 2;
            if (Compare(_entries[index], _entries[parentIndex]) >= 0) return;
            Swap(index, parentIndex);
            index = parentIndex;
        }
    }

    private void SiftDown(int index)
    {
        while (true)
        {
            int leftChildIndex = index * 2 + 1;
            if (leftChildIndex >= _entries.Count) return;

            int bestChildIndex = leftChildIndex;
            int rightChildIndex = leftChildIndex + 1;
            if (rightChildIndex < _entries.Count && Compare(_entries[rightChildIndex], _entries[leftChildIndex]) < 0)
                bestChildIndex = rightChildIndex;

            if (Compare(_entries[bestChildIndex], _entries[index]) >= 0) return;
            Swap(index, bestChildIndex);
            index = bestChildIndex;
        }
    }

    private void Swap(int firstIndex, int secondIndex)
    {
        HeapEntry temporary = _entries[firstIndex];
        _entries[firstIndex] = _entries[secondIndex];
        _entries[secondIndex] = temporary;
    }

    private static int Compare(HeapEntry left, HeapEntry right)
    {
        int comparison = left.F.CompareTo(right.F);
        if (comparison != 0) return comparison;

        comparison = left.H.CompareTo(right.H);
        if (comparison != 0) return comparison;

        comparison = right.G.CompareTo(left.G);
        if (comparison != 0) return comparison;

        return left.InsertionSequence.CompareTo(right.InsertionSequence);
    }

    private static void ValidateCost(double value, string parameterName)
    {
        if (!NumericGuard.IsFinite(value) || value < 0d)
            throw new ArgumentOutOfRangeException(parameterName, "Search costs must be finite and non-negative.");
    }

    private sealed class HeapEntry
    {
        public HeapEntry(T item, double f, double h, double g, long insertionSequence)
        {
            Item = item;
            F = f;
            H = h;
            G = g;
            InsertionSequence = insertionSequence;
        }

        public T Item { get; }
        public double F { get; }
        public double H { get; }
        public double G { get; }
        public long InsertionSequence { get; }
    }
}