/* PQ.C
xref:
input:
output: stdout
does    - priority queue (PQ)
        - handles several PQ's
        - simple implementation
        - fixed max queue size, can't resize (easy to update)
        - PQ items should have a value field v = priority
*/
/************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include "pq.h"
/***********************************************************/

static void  upheap    (PQITEM* q, int k);
static void  downheap  (PQITEM* q, int k, int n);
static void  pq_error  (char s[], int rc);

/***********************************************************/

static void  pq_error  (char s[], int rc)
/* pq error handler, exits if rc != 0 */
{
  fprintf(stderr, s);
  fprintf(stderr, "\n");
  if (rc) exit(rc);
}
/***********************************************************/

PQUEUE   pq_construct (int maxn)
/*
  Allocate a PQ of specified max size.
  If maxn < 1 returns NULL, no memory allocated, error.
  If not enough memory, returns NULL.
  Otherwise allocates memory for PQ descriptor and data.
*/
{
  PQUEUE p=NULL;

  if (maxn < 1) {
    pq_error ("pq_construct: maxn < 1", 1);
    return p;
  }

  p = (PQUEUE) malloc((size_t) sizeof(*p));
  if (!p) {
    pq_error ("pq_construct: insufficient memory", 1);
    return p;
  }

  p->maxn = maxn;
  p->n    = 0;

  p->q = (PQITEM*) malloc((size_t) sizeof(PQITEM)*(2+maxn));

  if (!(p->q)) {
    pq_error ("pq_construct: insufficient memory", 1);
    free(p);
    return NULL;
  }
  return p;
}
/***********************************************************/

void   pq_destruct (PQUEUE p)

/*   release memory allocated for a PQ  */

{
  free (p->q);
  free (p);
}
/***********************************************************/

int     pq_insert    (PQUEUE p, PQITEM x)

/* insert x into the queue p */
/* returns 1 if successful, 0 if failed (full queue) */
{
  if (p->n == p->maxn) {
    pq_error ("pq_insert: Full queue", 1);
    return 0;
  }
  p->q[++ (p->n)] = x;
  upheap(p->q, p->n);
  return 1;
}
/***********************************************************/

PQITEM pq_remove    (PQUEUE p)

/* remove the top item from the queue */
{
  PQITEM x;

  if (p->n == 0) {
    pq_error ("pq_remove: Empty queue, undefined return value", 1);
  }
  x = p->q[1];
  p->q[1] = p->q[p->n --];
  downheap(p->q, 1, p->n);
  return x;
}
/***********************************************************/

PQITEM pq_replace   (PQUEUE p, PQITEM x)

/* remove the top item from the queue */
/* replace it with x */
{
  if (p->n == 0) {
    pq_error ("pq_remove: Empty queue, undefined return value", 1);
  }
  p->q[0] = x;
  downheap(p->q, 0, p->n);
  return p->q[0];
}
/***********************************************************/

static void  upheap  (PQITEM* q, int k)

/* restore the heap condition */

{
  double v;
  PQITEM qk;

  qk = q[k];
  v = qk.v;
  q[0].v = DBL_MAX;
  while (q[k/2].v <= v) {
    q[k] = q[k/2];
    k /= 2;
  }
  q[k] = qk;
}

/***********************************************************/

static void  downheap  (PQITEM* q, int k, int n)

/* restore the heap condition, n = queue size */

{
  double v;
  int j;
  PQITEM qk;

  qk = q[k];
  v = qk.v;
  while (k <= n/2) {
    j = k + k;
    if (j < n && q[j].v < q[j+1].v) j++;
    if (v >= q[j].v) break;
    q[k] = q[j];
    k = j;
  }
  q[k] = qk;
}

/***********************************************************/