Added an optimization to A * p_old