cleaned up pathfinding, added more tests