Fixing Dijkstra algorithm result, and test