Algorithms: Update Dijkstra implementation