Add HEAP (heap.h, heap.cpp, heaptest.cpp)