text: #include <freeradius-devel/ident.h>
  RCSID("$Id$")
  
  #include <freeradius-devel/libradius.h>
  #include <freeradius-devel/heap.h>
  
  /*
   *	A heap entry is made of a pointer to the object, which
   *	contains the key.  The heap itself is an array of pointers.
   *
   *	Heaps normally support only ordered insert, and extraction
   *	of the minimum element.  The heap entry can contain an "int"
   *	field that holds the entries position in the heap.  The offset
   *	of the field is held inside of the heap structure.
   */
  
  struct fr_heap_t {
  	int size;
  	int num_elements;
  	size_t offset;
  	fr_heap_cmp_t cmp;
  	void **p;
  };
  
  /*
   *	First node in a heap is element 0. Children of i are 2i+1 and
   *	2i+2.  These macros wrap the logic, so the code is more
   *	descriptive.
   */
  #define HEAP_PARENT(x) ( ( (x) - 1 ) / 2 )
  #define HEAP_LEFT(x) ( 2*(x) + 1 )
  #define HEAP_RIGHT(x) ( 2*(x) + 2 )
  #define	HEAP_SWAP(a, b) { void *_tmp = a; a = b; b = _tmp; }
  
  static int fr_heap_bubble(fr_heap_t *hp, int child);
  
  void fr_heap_delete(fr_heap_t *hp)
  {
  	if (!hp) return;
  
  	free(hp->p);
  	free(hp);
  }
  
  fr_heap_t *fr_heap_create(fr_heap_cmp_t cmp, size_t offset)
  {       
  	fr_heap_t *fh;
  
  	if (!cmp) return NULL;
  
  	fh = malloc(sizeof(*fh));
  	if (!fh) return NULL;
  
  	memset(fh, 0, sizeof(*fh));
  
  	fh->size = 2048;
  	fh->p = malloc(sizeof(*(fh->p)) * fh->size);
  	if (!fh->p) {
  		free(fh);
  		return NULL;
  	}
  
  	fh->cmp = cmp;
  	fh->offset = offset;
  
  	return fh;
  }
  
  /*
   *	Insert element in heap. Normally, p != NULL, we insert p in a
   *	new position and bubble up. If p == NULL, then the element is
   *	already in place, and key is the position where to start the
   *	bubble-up.
   *
   *	Returns 1 on failure (cannot allocate new heap entry)
   *
   *	If offset > 0 the position (index, int) of the element in the
   *	heap is also stored in the element itself at the given offset
   *	in bytes.
   */
  #define SET_OFFSET(heap, node) \
      if (heap->offset) \
  	    *((int *)(((uint8_t *)heap->p[node]) + heap->offset)) = node
  
  /*
   *	RESET_OFFSET is used for sanity checks. It sets offset to an
   *	invalid value.
   */
  #define RESET_OFFSET(heap, node) \
      if (heap->offset) \
  	    *((int *)(((uint8_t *)heap->p[node]) + heap->offset)) = -1
  
  int fr_heap_insert(fr_heap_t *hp, void *data)
  {   
  	int child = hp->num_elements;
  	
  	/*
  	 *	Heap is full.  Double it's size.
  	 */
  	if (child == hp->size) {
  		void **p;
  		
  		p = malloc(2 * hp->size * sizeof(*p));
  		if (!p) return 0;
  		
  		memcpy(p, hp->p, sizeof(*p) * hp->size);
  		free(hp->p);
  		hp->p = p;
  		hp->size *= 2;
  	}
  
  	hp->p[child] = data;
  	hp->num_elements++;
  	
  	return fr_heap_bubble(hp, child);
  }
  
  
  static int fr_heap_bubble(fr_heap_t *hp, int child)
  {
  	/*
  	 *	Bubble up the element.
  	 */
  	while (child > 0) {
  		int parent = HEAP_PARENT(child);
  		
  		/*
  		 *	Parent is smaller than the child.  We're done.
  		 */
  		if (hp->cmp(hp->p[parent], hp->p[child]) < 0) break;
  		
  		/*
  		 *	Child is smaller than the parent, repeat.
  		 */
  		HEAP_SWAP(hp->p[child], hp->p[parent]);
  		SET_OFFSET(hp, child);
  		child = parent;
  	}
  	SET_OFFSET(hp, child);
  
  	return 1;
  }
  
  
  /*
   *	Remove the top element, or object.
   */
  int fr_heap_extract(fr_heap_t *hp, void *data)
  {  
  	int child, parent;
  	int max;
  
  	if (!hp || (hp->num_elements == 0)) return 0;
  	
  	max = hp->num_elements - 1;
  	
  	/*
  	 *	Extract element.  Default is the first one.
  	 */
  	if (!data) {
  		parent = 0;
  
  	} else {		/* extract from the middle */
  		if (!hp->offset) return 0;
  
  		parent = *((int *)(((uint8_t *)data) + hp->offset));
  
  		/*
  		 *	Out of bounds.
  		 */
  		if (parent < 0 || parent >= hp->num_elements) return 0;
  	}
  
  	RESET_OFFSET(hp, parent);
  	child = HEAP_LEFT(parent);
  	while (child <= max) {
  		/*
  		 *	Maybe take the right child.
  		 */
  		if ((child != max) &&
  		    (hp->cmp(hp->p[child + 1], hp->p[child]) < 0)) {
  			child = child + 1;
  		}
  		hp->p[parent] = hp->p[child];
  		SET_OFFSET(hp, parent);
  		parent = child;
  		child = HEAP_LEFT(child);
  	}
  	hp->num_elements--;
  
  	/*
  	 *	We didn't end up at the last element in the heap.
  	 *	This element has to be re-inserted.
  	 */
  	if (parent != max) {
  		/*
  		 *	Fill hole with last entry and bubble up,
  		 *	reusing the insert code
  		 */
  		hp->p[parent] = hp->p[max];
  		return fr_heap_bubble(hp, parent);
  	}
  
  	return 1;
  }
  
  
  void *fr_heap_peek(fr_heap_t *hp)
  {
  	if (!hp || (hp->num_elements == 0)) return NULL;
  
  	/*
  	 *	If this is NULL, we have a problem.
  	 */
  	return hp->p[0];
  }
  
  int fr_heap_num_elements(fr_heap_t *hp)
  {
  	if (!hp) return 0;
  
  	return hp->num_elements;
  }
  
  
  #ifdef TESTING
  /*
   *  cc -g -DTESTING -I .. heap.c -o heap
   *
   *  ./heap
   */
  
  #include <stdio.h>
  #include <stdlib.h>
  
  
  static int heap_cmp(const void *a, const void *b)
  {
  	return *(int *)a - *(int *) b;
  
  }
  
  
  int main(int argc, char **arg)
  {
  	fr_heap_t *hp;
  	int i, array[1024];
  
  	hp = fr_heap_create(heap_cmp, 0);
  	if (!hp) {
  		fprintf(stderr, "Failed creating heap!\n");
  		exit(1);
  	}
  
  	for (i = 0; i < 1024; i++) {
  		array[i] = (i * 257) % 65537;
  		if (!fr_heap_insert(hp, &array[i])) {
  			fprintf(stderr, "Failed inserting %d\n", i);
  			exit(1);
  		}
  	}
  
  	for (i = 0; i < 1024; i++) {
  		int *p = fr_heap_peek(hp);
  
  		if (!p) {
  			fprintf(stderr, "Failed peeking %d\n", i);
  			exit(1);
  		}
  
  		printf("%d\t%d\n", i, *p);
  
  		if (!fr_heap_extract(hp, NULL)) {
  			fprintf(stderr, "Failed extracting %d\n", i);
  			exit(1);
  		}
  	}
  
  	fr_heap_delete(hp);
  	
  	return 0;
  }
  #endif
  