Fixed Dijkstra implementation