Pathfinding half working