Finished Pathfinder implementation