First take on Dijkstra with priority queue
Working version of dijkstra with priority queue.