DijsktrasAlgorithm

using System.Collections.Generic;
using PriorityQueue;

namespace Pathfinding
{
    public class DijsktraAlgorithm
    {
        Dictionary<Node, int> totalCost = new Dictionary<Node, int>();
        Dictionary<Node, Node> prevNodes = new Dictionary<Node, Node>();
        PriorityQueue<Node> queue = new PriorityQueue<Node>();
        HashSet<Node> visited = new HashSet<Node>();

        public DijsktraAlgorithm(){}

        public List<object> RunAlgorithm(Graph graph, Node start)
        {
            totalCost.Clear();
            prevNodes.Clear();
            queue.Clear();
            visited.Clear();

            totalCost.Add(start, 0);
            queue.Enqueue(start, 0);

            foreach (Node n in graph.nodes)
            {
                if (n != start)
                {
                    totalCost.Add(n, int.MaxValue);
                }
            }

            while (queue.Count > 0)
            {
                if (queue.Count == 0)
                {

                    break;
                }
                Node newSmallest = queue.Dequeue();

                visited.Add(newSmallest);

                foreach (Node neighbor in newSmallest.neighbors)
                {
                    if (!queue.Contains(neighbor) && !visited.Contains(neighbor))
                    {
                        int priority = (int)UnityEngine.Vector3.Distance(newSmallest.transform.position,
                            neighbor.transform.position);

                        queue.Enqueue(neighbor, priority);
                    }
                }

                foreach (Node neighbor in newSmallest.neighbors)
                {
                    if (!visited.Contains(neighbor))
                    {
                        int altPath = totalCost[newSmallest] +
                            (int)UnityEngine.Vector3.Distance(newSmallest.transform.position,
                            neighbor.transform.position);

                        if (altPath < totalCost[neighbor])
                        {
                            if (!totalCost.ContainsKey(neighbor))
                            {
                                totalCost.Add(neighbor, altPath);
                            }
                            else
                            {
                                totalCost[neighbor] = altPath;
                            }

                            if (!prevNodes.ContainsKey(neighbor))
                            {
                                prevNodes.Add(neighbor, newSmallest);
                            }
                            else
                            {
                                prevNodes[neighbor] = newSmallest;
                            }

                            if (queue.Contains(neighbor))
                            {
                                queue.UpdatePriority(neighbor, altPath);
                            }
                        }
                    }
                }
            }

            List<object> result = new List<object>();

            result.Add(totalCost); result.Add(prevNodes);

            return result;

        }
    }
}