Fix Dijkstra + add tests