Adjusted Dijkstra algorithm to be more correct.