Some refactoring in the pathfinding algorithm