Bug fixes/re-write of Dijkstra