added tests for pathfinder, fixed bugs

It should work now!