// see license file for original license.

#ifndef tools_glutess_priorityq
#define tools_glutess_priorityq

#include <climits>		/* LONG_MAX */
#include "memalloc"

/* Include all the code for the regular heap-based queue here. */

/////////////////////////////////////////////////////////////////
//#include "priorityq-heap.ic"
//#include "priorityq-heap"

/* Use #define's so that another heap implementation can use this one */

#define PQkey			PQHeapKey
#define PQhandle		PQHeapHandle
#define PriorityQ		PriorityQHeap

#define pqNewPriorityQ(leq)	__gl_pqHeapNewPriorityQ(leq)
#define pqDeletePriorityQ(pq)	__gl_pqHeapDeletePriorityQ(pq)

/* The basic operations are insertion of a new key (pqInsert),
 * and examination/extraction of a key whose value is minimum
 * (pqMinimum/pqExtractMin).  Deletion is also allowed (pqDelete);
 * for this purpose pqInsert returns a "handle" which is supplied
 * as the argument.
 *
 * An initial heap may be created efficiently by calling pqInsert
 * repeatedly, then calling pqInit.  In any case pqInit must be called
 * before any operations other than pqInsert are used.
 *
 * If the heap is empty, pqMinimum/pqExtractMin will return a NULL key.
 * This may also be tested with pqIsEmpty.
 */
#define pqInit(pq)		__gl_pqHeapInit(pq)
#define pqInsert(pq,key)	__gl_pqHeapInsert(pq,key)
#define pqMinimum(pq)		__gl_pqHeapMinimum(pq)
#define pqExtractMin(pq)	__gl_pqHeapExtractMin(pq)
#define pqDelete(pq,handle)	__gl_pqHeapDelete(pq,handle)
#define pqIsEmpty(pq)		__gl_pqHeapIsEmpty(pq)

/* Since we support deletion the data structure is a little more
 * complicated than an ordinary heap.  "nodes" is the heap itself;
 * active nodes are stored in the range 1..pq->size.  When the
 * heap exceeds its allocated size (pq->max), its size doubles.
 * The children of node i are nodes 2i and 2i+1.
 *
 * Each node stores an index into an array "handles".  Each handle
 * stores a key, plus a pointer back to the node which currently
 * represents that key (ie. nodes[handles[i].node].handle == i).
 */

typedef void *PQkey;
typedef long PQhandle;
typedef struct PriorityQ PriorityQ;

typedef struct { PQhandle handle; } PQnode;
typedef struct { PQkey key; PQhandle node; } PQhandleElem;

struct PriorityQ {
  PQnode	*nodes;
  PQhandleElem	*handles;
  long		size, max;
  PQhandle	freeList;
  int		initialized;
  int		(*leq)(PQkey key1, PQkey key2);
};
  
#define __gl_pqHeapMinimum(pq)	((pq)->handles[(pq)->nodes[1].handle].key)
#define __gl_pqHeapIsEmpty(pq)	((pq)->size == 0)

/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
//#define INIT_SIZE 32
inline long INIT_SIZE() {
  static const long s_value = 32;
  return s_value;
}

/* Violates modularity, but a little faster */
#include "geom"
#define LEQ(x,y)	VertLeq((GLUvertex *)x, (GLUvertex *)y)

/* really __gl_pqHeapNewPriorityQ */
inline PriorityQ *pqNewPriorityQ( int (*leq)(PQkey key1, PQkey key2) )
{
  PriorityQ *pq = (PriorityQ *)memAlloc( sizeof( PriorityQ ));
  if (pq == NULL) return NULL;

  pq->size = 0;
  pq->max = INIT_SIZE();
  pq->nodes = (PQnode *)memAlloc( (INIT_SIZE() + 1) * sizeof(pq->nodes[0]) );
  if (pq->nodes == NULL) {
     memFree(pq);
     return NULL;
  }

  pq->handles = (PQhandleElem *)memAlloc( (INIT_SIZE() + 1) * sizeof(pq->handles[0]) );
  if (pq->handles == NULL) {
     memFree(pq->nodes);
     memFree(pq);
     return NULL;
  }

  pq->initialized = TOOLS_GLU_FALSE;
  pq->freeList = 0;
  pq->leq = leq;

  pq->nodes[1].handle = 1;	/* so that Minimum() returns NULL */
  pq->handles[1].key = NULL;
  return pq;
}

/* really __gl_pqHeapDeletePriorityQ */
inline void pqDeletePriorityQ( PriorityQ *pq )
{
  memFree( pq->handles );
  memFree( pq->nodes );
  memFree( pq );
}


inline/*static*/ void static_FloatDown( PriorityQ *pq, long curr )
{
  PQnode *n = pq->nodes;
  PQhandleElem *h = pq->handles;
  PQhandle hCurr, hChild;
  long child;

  hCurr = n[curr].handle;
  for( ;; ) {
    child = curr << 1;
    if( child < pq->size && LEQ( h[n[child+1].handle].key,
				 h[n[child].handle].key )) {
      ++child;
    }

    assert(child <= pq->max);

    hChild = n[child].handle;
    if( child > pq->size || LEQ( h[hCurr].key, h[hChild].key )) {
      n[curr].handle = hCurr;
      h[hCurr].node = curr;
      break;
    }
    n[curr].handle = hChild;
    h[hChild].node = curr;
    curr = child;
  }
}


inline/*static*/ void static_FloatUp( PriorityQ *pq, long curr )
{
  PQnode *n = pq->nodes;
  PQhandleElem *h = pq->handles;
  PQhandle hCurr, hParent;
  long parent;

  hCurr = n[curr].handle;
  for( ;; ) {
    parent = curr >> 1;
    hParent = n[parent].handle;
    if( parent == 0 || LEQ( h[hParent].key, h[hCurr].key )) {
      n[curr].handle = hCurr;
      h[hCurr].node = curr;
      break;
    }
    n[curr].handle = hParent;
    h[hParent].node = curr;
    curr = parent;
  }
}

/* really __gl_pqHeapInit */
inline void pqInit( PriorityQ *pq )
{
  long i;

  /* This method of building a heap is O(n), rather than O(n lg n). */

  for( i = pq->size; i >= 1; --i ) {
    static_FloatDown( pq, i );
  }
  pq->initialized = TOOLS_GLU_TRUE;
}

/* really __gl_pqHeapInsert */
/* returns LONG_MAX iff out of memory */
inline PQhandle pqInsert( PriorityQ *pq, PQkey keyNew )
{
  long curr;
  PQhandle free;

  curr = ++ pq->size;
  if( (curr*2) > pq->max ) {
    PQnode *saveNodes= pq->nodes;
    PQhandleElem *saveHandles= pq->handles;

    /* If the heap overflows, double its size. */
    pq->max <<= 1;
    pq->nodes = (PQnode *)memRealloc( pq->nodes, 
				     (size_t) 
				     ((pq->max + 1) * sizeof( pq->nodes[0] )));
    if (pq->nodes == NULL) {
       pq->nodes = saveNodes;	/* restore ptr to free upon return */
       return LONG_MAX;
    }
    pq->handles = (PQhandleElem *)memRealloc( pq->handles,
			                     (size_t)
			                      ((pq->max + 1) * 
					       sizeof( pq->handles[0] )));
    if (pq->handles == NULL) {
       pq->handles = saveHandles; /* restore ptr to free upon return */
       return LONG_MAX;
    }
  }

  if( pq->freeList == 0 ) {
    free = curr;
  } else {
    free = pq->freeList;
    pq->freeList = pq->handles[free].node;
  }

  pq->nodes[curr].handle = free;
  pq->handles[free].node = curr;
  pq->handles[free].key = keyNew;

  if( pq->initialized ) {
    static_FloatUp( pq, curr );
  }
  assert(free != LONG_MAX);
  return free;
}

/* really __gl_pqHeapExtractMin */
inline PQkey pqExtractMin( PriorityQ *pq )
{
  PQnode *n = pq->nodes;
  PQhandleElem *h = pq->handles;
  PQhandle hMin = n[1].handle;
  PQkey min = h[hMin].key;

  if( pq->size > 0 ) {
    n[1].handle = n[pq->size].handle;
    h[n[1].handle].node = 1;

    h[hMin].key = NULL;
    h[hMin].node = pq->freeList;
    pq->freeList = hMin;

    if( -- pq->size > 0 ) {
      static_FloatDown( pq, 1 );
    }
  }
  return min;
}

/* really __gl_pqHeapDelete */
inline void pqDelete( PriorityQ *pq, PQhandle hCurr )
{
  PQnode *n = pq->nodes;
  PQhandleElem *h = pq->handles;
  long curr;

  assert( hCurr >= 1 && hCurr <= pq->max && h[hCurr].key != NULL );

  curr = h[hCurr].node;
  n[curr].handle = n[pq->size].handle;
  h[n[curr].handle].node = curr;

  if( curr <= -- pq->size ) {
    if( curr <= 1 || LEQ( h[n[curr>>1].handle].key, h[n[curr].handle].key )) {
      static_FloatDown( pq, curr );
    } else {
      static_FloatUp( pq, curr );
    }
  }
  h[hCurr].key = NULL;
  h[hCurr].node = pq->freeList;
  pq->freeList = hCurr;
}

/* Now redefine all the function names to map to their "Sort" versions. */

/////////////////////////////////////////////////////////////////
//#include "priorityq-sort"

#undef PQkey
#undef PQhandle
#undef PriorityQ
#undef pqNewPriorityQ
#undef pqDeletePriorityQ
#undef pqInit
#undef pqInsert
#undef pqMinimum
#undef pqExtractMin
#undef pqDelete
#undef pqIsEmpty

/* Use #define's so that another heap implementation can use this one */

#define PQkey			PQSortKey
#define PQhandle		PQSortHandle
#define PriorityQ		PriorityQSort

#define pqNewPriorityQ(leq)	__gl_pqSortNewPriorityQ(leq)
#define pqDeletePriorityQ(pq)	__gl_pqSortDeletePriorityQ(pq)

/* The basic operations are insertion of a new key (pqInsert),
 * and examination/extraction of a key whose value is minimum
 * (pqMinimum/pqExtractMin).  Deletion is also allowed (pqDelete);
 * for this purpose pqInsert returns a "handle" which is supplied
 * as the argument.
 *
 * An initial heap may be created efficiently by calling pqInsert
 * repeatedly, then calling pqInit.  In any case pqInit must be called
 * before any operations other than pqInsert are used.
 *
 * If the heap is empty, pqMinimum/pqExtractMin will return a NULL key.
 * This may also be tested with pqIsEmpty.
 */
#define pqInit(pq)		__gl_pqSortInit(pq)
#define pqInsert(pq,key)	__gl_pqSortInsert(pq,key)
#define pqMinimum(pq)		__gl_pqSortMinimum(pq)
#define pqExtractMin(pq)	__gl_pqSortExtractMin(pq)
#define pqDelete(pq,handle)	__gl_pqSortDelete(pq,handle)
#define pqIsEmpty(pq)		__gl_pqSortIsEmpty(pq)


/* Since we support deletion the data structure is a little more
 * complicated than an ordinary heap.  "nodes" is the heap itself;
 * active nodes are stored in the range 1..pq->size.  When the
 * heap exceeds its allocated size (pq->max), its size doubles.
 * The children of node i are nodes 2i and 2i+1.
 *
 * Each node stores an index into an array "handles".  Each handle
 * stores a key, plus a pointer back to the node which currently
 * represents that key (ie. nodes[handles[i].node].handle == i).
 */

typedef PQHeapKey PQkey;
typedef PQHeapHandle PQhandle;
typedef struct PriorityQ PriorityQ;

struct PriorityQ {
  PriorityQHeap	*heap;
  PQkey		*keys;
  PQkey		**order;
  PQhandle	size, max;
  int		initialized;
  int		(*leq)(PQkey key1, PQkey key2);
};
  
/* really __gl_pqSortNewPriorityQ */
inline PriorityQ *pqNewPriorityQ( int (*leq)(PQkey key1, PQkey key2) )
{
  PriorityQ *pq = (PriorityQ *)memAlloc( sizeof( PriorityQ ));
  if (pq == NULL) return NULL;

  pq->heap = __gl_pqHeapNewPriorityQ( leq );
  if (pq->heap == NULL) {
     memFree(pq);
     return NULL;
  }

  pq->keys = (PQHeapKey *)memAlloc( INIT_SIZE() * sizeof(pq->keys[0]) );
  if (pq->keys == NULL) {
     __gl_pqHeapDeletePriorityQ(pq->heap);
     memFree(pq);
     return NULL;
  }

  pq->size = 0;
  pq->max = INIT_SIZE();
  pq->initialized = TOOLS_GLU_FALSE;
  pq->leq = leq;
  return pq;
}

/* really __gl_pqSortDeletePriorityQ */
inline void pqDeletePriorityQ( PriorityQ *pq )
{
  assert(pq != NULL); 
  if (pq->heap != NULL) __gl_pqHeapDeletePriorityQ( pq->heap );
  if (pq->order != NULL) memFree( pq->order );
  if (pq->keys != NULL) memFree( pq->keys );
  memFree( pq );
}


#define LT(x,y)		(! LEQ(y,x))
#define GT(x,y)		(! LEQ(x,y))
//#define pq_Swap(a,b)	if(1){PQkey *tmp = *a; *a = *b; *b = tmp;}else
#define pq_Swap(a,b)	do{PQkey *tmp = *a; *a = *b; *b = tmp;} while(false)

/* really __gl_pqSortInit */
inline int pqInit( PriorityQ *pq )
{
  PQkey **p, **r, **i, **j, *piv;
  struct { PQkey **p, **r; } Stack[50], *top = Stack;
  unsigned long seed = 2016473283;

  /* Create an array of indirect pointers to the keys, so that we
   * the handles we have returned are still valid.
   */
/*
  pq->order = (PQHeapKey **)memAlloc( (size_t)
                                  (pq->size * sizeof(pq->order[0])) );
*/
  pq->order = (PQHeapKey **)memAlloc( (size_t)
                                  ((pq->size+1) * sizeof(pq->order[0])) );
/* the previous line is a patch to compensate for the fact that IBM */
/* machines return a null on a malloc of zero bytes (unlike SGI),   */
/* so we have to put in this defense to guard against a memory      */
/* fault four lines down. from fossum@austin.ibm.com.               */
  if (pq->order == NULL) return 0;

  p = pq->order;
  r = p + pq->size - 1;
  for( piv = pq->keys, i = p; i <= r; ++piv, ++i ) {
    *i = piv;
  }

  /* Sort the indirect pointers in descending order,
   * using randomized Quicksort
   */
  top->p = p; top->r = r; ++top;
  while( --top >= Stack ) {
    p = top->p;
    r = top->r;
    while( r > p + 10 ) {
      seed = seed * 1539415821 + 1;
      i = p + seed % (r - p + 1);
      piv = *i;
      *i = *p;
      *p = piv;
      i = p - 1;
      j = r + 1;
      do {
	do { ++i; } while( GT( **i, *piv ));
	do { --j; } while( LT( **j, *piv ));
	pq_Swap( i, j );
      } while( i < j );
      pq_Swap( i, j );	/* Undo last swap */
      if( i - p < r - j ) {
	top->p = j+1; top->r = r; ++top;
	r = i-1;
      } else {
	top->p = p; top->r = i-1; ++top;
	p = j+1;
      }
    }
    /* Insertion sort small lists */
    for( i = p+1; i <= r; ++i ) {
      piv = *i;
      for( j = i; j > p && LT( **(j-1), *piv ); --j ) {
	*j = *(j-1);
      }
      *j = piv;
    }
  }
  pq->max = pq->size;
  pq->initialized = TOOLS_GLU_TRUE;
  __gl_pqHeapInit( pq->heap );	/* always succeeds */

#ifndef NDEBUG
  p = pq->order;
  r = p + pq->size - 1;
  for( i = p; i < r; ++i ) {
    assert( LEQ( **(i+1), **i ));
  }
#endif

  return 1;
}

/* really __gl_pqSortInsert */
/* returns LONG_MAX iff out of memory */ 
inline PQhandle pqInsert( PriorityQ *pq, PQkey keyNew )
{
  long curr;

  if( pq->initialized ) {
    return __gl_pqHeapInsert( pq->heap, keyNew );
  }
  curr = pq->size;
  if( ++ pq->size >= pq->max ) {
    PQkey *saveKey= pq->keys;

    /* If the heap overflows, double its size. */
    pq->max <<= 1;
    pq->keys = (PQHeapKey *)memRealloc( pq->keys, 
	 	                        (size_t)
	                                 (pq->max * sizeof( pq->keys[0] )));
    if (pq->keys == NULL) {	
       pq->keys = saveKey;	/* restore ptr to free upon return */
       return LONG_MAX;
    }
  }
  assert(curr != LONG_MAX);	
  pq->keys[curr] = keyNew;

  /* Negative handles index the sorted array. */
  return -(curr+1);
}

/* really __gl_pqSortExtractMin */
inline PQkey pqExtractMin( PriorityQ *pq )
{
  PQkey sortMin, heapMin;

  if( pq->size == 0 ) {
    return __gl_pqHeapExtractMin( pq->heap );
  }
  sortMin = *(pq->order[pq->size-1]);
  if( ! __gl_pqHeapIsEmpty( pq->heap )) {
    heapMin = __gl_pqHeapMinimum( pq->heap );
    if( LEQ( heapMin, sortMin )) {
      return __gl_pqHeapExtractMin( pq->heap );
    }
  }
  do {
    -- pq->size;
  } while( pq->size > 0 && *(pq->order[pq->size-1]) == NULL );
  return sortMin;
}

/* really __gl_pqSortMinimum */
inline PQkey pqMinimum( PriorityQ *pq )
{
  PQkey sortMin, heapMin;

  if( pq->size == 0 ) {
    return __gl_pqHeapMinimum( pq->heap );
  }
  sortMin = *(pq->order[pq->size-1]);
  if( ! __gl_pqHeapIsEmpty( pq->heap )) {
    heapMin = __gl_pqHeapMinimum( pq->heap );
    if( LEQ( heapMin, sortMin )) {
      return heapMin;
    }
  }
  return sortMin;
}

/* really __gl_pqSortIsEmpty */
inline int pqIsEmpty( PriorityQ *pq )
{
  return (pq->size == 0) && __gl_pqHeapIsEmpty( pq->heap );
}

/* really __gl_pqSortDelete */
inline void pqDelete( PriorityQ *pq, PQhandle curr )
{
  if( curr >= 0 ) {
    __gl_pqHeapDelete( pq->heap, curr );
    return;
  }
  curr = -(curr+1);
  assert( curr < pq->max && pq->keys[curr] != NULL );

  pq->keys[curr] = NULL;
  while( pq->size > 0 && *(pq->order[pq->size-1]) == NULL ) {
    -- pq->size;
  }
}

#endif