Some refactoring and Pathfinding