Add function: insert a new node to heap