MADS: Refactored pathfinder into it's own class