Fixed Dijkstra algorithm