PriorityQueue.cs

// A Generic Min Heap Priority Queue for Unity that uses supplied float values for its ordering.
using System.Collections.Generic;
using UnityEngine;

public class PriorityQueue<T> where T : class{

    struct ValueItem {
        public T item;
        public float value;
        public ValueItem(T i, float v) { 
            item = i;
            value = v;
        }
    }

    private List<ValueItem> items = new List<ValueItem>();
    private int elements = 0;
    float last_value = 0.0f;

    public int size() {
        return elements;
    }

    public void clear() {
        items.Clear();
        elements = 0;
    }
    
    public void insert(T new_item, float new_value) {
        int i = elements;
        ValueItem vi = new ValueItem(new_item, new_value);
        items.Add(vi);
        while (i > 0 && items[(i - 1) / 2].value >  new_value) {
            items[i] = items[(i - 1) / 2];
            i = (i - 1) / 2;
        }
        items[i] = vi;
        elements++;
    }

    public T peek() {
        if (elements == 0) {
            return null;
        }
        return items[0].item;
    }

    public T pop() {
        if (elements== 0) {
            return null;
        }
        // Get the first item
        ValueItem rslt = items[0];
        // Get the last item and bubble it down.
        ValueItem tmp = items[elements - 1];
        items.RemoveAt(elements - 1);
        elements--;
        if (elements > 0) {
            int i = 0;
            while (i < elements/ 2) {
                int j = (2 * i) + 1;
                if ((j < elements - 1) && items[j].value >  items[j + 1].value) {
                    ++j;
                }
                if (items[j].value >  tmp.value) {
                    break;
                }
                items[i] = items[j];
                i = j;
            }
            items[i] = tmp;
        }
        last_value = rslt.value;
        return rslt.item;
    }

    public float lastPopValue() {
        return last_value;
    }

    public static void test() {
        PriorityQueue<string> queue = new PriorityQueue<string>();
        Debug.Log(queue.size() + " == 0 ");
        queue.insert("two", 2.0f);
        queue.insert("one", 1.0f);
        queue.insert("three", 3.0f);
        Debug.Log(queue.size() + " == 3 ");
        queue.insert("five", 5.0f);
        queue.insert("four", 4.0f);

        Debug.Log(queue.size() + " == 5 ");
        Debug.Log("12345");
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.size() + " == 2 ");
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.size() + " == 0 ");

        queue.insert("five", 5.0f);
        queue.insert("one", 1.0f);
        queue.insert("three", 3.0f);
        queue.insert("four", 4.0f);
        queue.insert("two", 2.0f);
        Debug.Log("12");
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.size() + " == 3 ");

        queue.insert("one", 1.0f);
        queue.insert("six", 6.0f);
        queue.insert("two", 2.0f);
        Debug.Log(queue.size() + " == 6 ");
        Debug.Log("123456");
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.pop());
        Debug.Log(queue.size() + " == 0 ");
    }
}