Updated pathfinder
Updated Pathfinder algorithm