Add basic heap functionality.

- Add internal functions for percolating nodes up and down.
- Add function for inserting items into the heap.
- Add functions for popping minimum and maximum element.
- Added test code for the above functionality.
- Add function for printing heap, for debug purposes.