Update src/kheap.c

Adding heap implementation from JamesM's tutorial