Add Heap code

Use template (simple type only)