Dijkstra's algorithm working, verified correctness