Depending on how performance critical this is, we could maybe reuse parts of this https://github.com/ncloudioj/cyheap