Pathfinding unit tests WIP