PotatoPlan/Game1/Sources/Pathing/A-Star/PathSaver/PriorityQueue.cs

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Graphics;

class PriorityQueue
{
    public List<Path> list;
    public int Count { get { return list.Count; } }

    public PriorityQueue()
    {
        list = new List<Path>();
    }

    public PriorityQueue(int count)
    {
        list = new List<Path>(count);
    }


    public void Enqueue(Path x)
    {
        list.Add(x);
        int i = Count - 1;

        while (i > 0)
        {
            int p = (i - 1) / 2;
            if (list[p].getEfficency() <= x.getEfficency()) break;

            list[i] = list[p];
            i = p;
        }

        if (Count > 0) list[i] = x;
    }

    public void Dequeue()
    {
        Path min = Peek();
        Path root = list[Count - 1];
        list.RemoveAt(Count - 1);

        int i = 0;
        while (i * 2 + 1 < Count)
        {
            int a = i * 2 + 1;
            int b = i * 2 + 2;
            int c = b < Count && list[b].getEfficency() < list[a].getEfficency() ? b : a;

            if (list[c].getEfficency() >= root.getEfficency()) break;
            list[i] = list[c];
            i = c;
        }

        if (Count > 0) list[i] = root;
    }

    public Path Peek()
    {
        if (Count == 0) throw new InvalidOperationException("Queue is empty.");
        return list[0];
    }

    public void Clear()
    {
        list.Clear();
    }
}