Author's alg completed. Modifying Dijkstra