Fixing bug in Pathfinder algorithm