Working version of dijkstra with priority queue.