Pathfinding test working