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<Nodes> list;
    public int Count { get { return list.Count; } }

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

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


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

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

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

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

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

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

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

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

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

    public Boolean Exists(Vector2 coordinates)
    {
        if (Count == 0)
            return false;
        foreach(Nodes node in list)
        {
            if (node.getCords() == coordinates)
                return true;
        }
        return false;
    }


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