Refactoring and pathfinding bugfixes