Updating dijkstra_shortest_path_to, seems buggy.
Using dijkstra, buggy.