Dijkstra: Shortest Reach 2 c++1