Worked on Dijkstra, seems to be working decently (with simple costs), but need to update unit tests
Merge branch 'Dijkstra' of https://github.com/jannickj/nabf into Dijkstra