Working shortest path algorithm, not optimal