TheriapolisV3/Theriapolis.Core/Util/BinaryHeap.cs

namespace Theriapolis.Core.Util;

/// <summary>
/// Min-heap priority queue used by AStarPathfinder.
/// Items with lower priority are extracted first.
/// Uses lazy deletion: does not support decrease-key; instead, duplicates are
/// inserted and stale entries are skipped at extraction time.
/// </summary>
public sealed class BinaryHeap<T>
{
    private struct Entry
    {
        public float Priority;
        public T     Item;
    }

    private Entry[] _heap;
    private int     _count;

    public BinaryHeap(int capacity = 1024)
    {
        _heap  = new Entry[capacity];
        _count = 0;
    }

    public int Count => _count;
    public bool IsEmpty => _count == 0;

    public void Clear() => _count = 0;

    public void Insert(T item, float priority)
    {
        if (_count == _heap.Length)
            Array.Resize(ref _heap, _heap.Length * 2);

        _heap[_count] = new Entry { Priority = priority, Item = item };
        BubbleUp(_count);
        _count++;
    }

    public (T Item, float Priority) Peek()
    {
        if (_count == 0) throw new InvalidOperationException("Heap is empty.");
        return (_heap[0].Item, _heap[0].Priority);
    }

    public T ExtractMin()
    {
        if (_count == 0) throw new InvalidOperationException("Heap is empty.");
        var result = _heap[0];
        _count--;
        if (_count > 0)
        {
            _heap[0] = _heap[_count];
            BubbleDown(0);
        }
        return result.Item;
    }

    private void BubbleUp(int i)
    {
        while (i > 0)
        {
            int parent = (i - 1) >> 1;
            if (_heap[parent].Priority <= _heap[i].Priority) break;
            (_heap[parent], _heap[i]) = (_heap[i], _heap[parent]);
            i = parent;
        }
    }

    private void BubbleDown(int i)
    {
        while (true)
        {
            int left  = (i << 1) + 1;
            int right = left + 1;
            int min   = i;

            if (left  < _count && _heap[left].Priority  < _heap[min].Priority) min = left;
            if (right < _count && _heap[right].Priority < _heap[min].Priority) min = right;

            if (min == i) break;
            (_heap[min], _heap[i]) = (_heap[i], _heap[min]);
            i = min;
        }
    }
}