Update/improve dijkstra, add real test