Almost done with heap, has a bug in free function