Finished Dijkstra's implementation