Refactoring: Utilities, Pathfinder

Reorganize code for easier usage