Add Pathfinder stuff
implement PathFinder first phase
merged branch pathfinder