Bug fix in optimized Dijkstra