Optimized Dijkstra to use one call per node