heap: Add insert function

Signed-off-by: Prashant Malani <pmalani@nvidia.com>