Refactoring: Grid and Pathfinder

Refactor Grid and Pathfinder into classes