LD50/Assets/Scripts/Priorityqueue.cs

using System;
using System.Collections.Generic;

public class PriorityQueue<T>
{
    private Func<T, T, int> comparer;

    public PriorityQueue(Func<T, T, int> comparer = null)
    {
        if (comparer == null)
        {
            comparer = (x, y) => Comparer<T>.Default.Compare(x, y);
        }
        this.comparer = comparer;
    }
    
    private readonly List<T> queue = new List<T>();

    private void MinHeapify(int i)
    {
        int left = 2 * i + 1;
        int right = 2 * i + 2;
        int smallest = i;
        if (left < queue.Count && comparer(queue[left], queue[smallest]) < 0)
        {
            smallest = left;
        }
        if (right < queue.Count && comparer(queue[right], queue[smallest]) < 0)
        {
            smallest = right;
        }
        if (smallest != i)
        {
            (queue[i], queue[smallest]) = (queue[smallest], queue[i]);
            MinHeapify(smallest);
        }
    }

    public void Insert(T item)
    {
        queue.Add(item);
        int ci = queue.Count - 1; // child index; start at end
        while (ci > 0)
        {
            int pi = (ci - 1) / 2; // parent index
            if (comparer(queue[ci], queue[pi]) >= 0) break; // child item is larger than (or equal) parent so we're done
            (queue[ci], queue[pi]) = (queue[pi], queue[ci]);
            ci = pi;
        }
    }
    
    public T Extract()
    {
        T item = queue[0];
        queue[0] = queue[queue.Count - 1];
        queue.RemoveAt(queue.Count - 1);
        MinHeapify(0);
        return item;
    }
    
    public int Count => queue.Count;

    public T Peek()
    {
        return queue[0];
    }
    
    public bool Contains(T item)
    {
        return queue.Contains(item);
    }
    
    public void Clear()
    {
        queue.Clear();
    }

    public bool IsEmpty()
    {
        return queue.Count == 0;
    }


}