Binary heap insertion working