Started working the pathfinder with new graph