Heap insertion algorithm