Separated the Geo parts into their own class
Fixed pathfinder, updated test_pathfinder