Correct heap add and remove conditions