Updated Pathfinder algorithm