Dijkstra's algorithm - C++ implementation - (high) efficient