Class-based pathfinder.py
Separated the Geo parts into their own class
Fixed pathfinder, updated test_pathfinder