#include <stdio.h>
#include <stdlib.h>

/* DPriorityQueue, fixed maximum size, minimal use of dynamic memory. */

/* As it stands, this priority queue implementation just stores data items
   of the type "example_data_item", which is defined in this file,
   and is only capable of holding a single integer. It is here as
   as example only. When you have defined your own data objects that
   are to be stored in the queue, delete the two lines below that say
   "typedef ... example_data_item", and in the following "#define" line,
   replace the name "example_data_item" with your own data object's type.
   Also delete the create_item function, which will be useless. This
   otherwise worthless section is only here so that an example will work.

   because priority queues need to know the priority of each item they hold,
   the data type must support a function "priority_of" which takes a pointer
   to the data type as its parameter and returns a float as its result. When
   you have your own "priority_of" function, remove the example one from below. */

typedef struct                                /* remove this */
{ int value; } example_data_item;

float priority_of(example_data_item *it)      /* replace this */
{ return ((float)(it->value)); }

#define item example_data_item                /* replace this */

item *create_item(int v)                      /* remove this */
{ item *x;
  x=(item *)malloc(sizeof(item));
  x->value=v;
  return (x); }

/* Sometimes you need to search an area of storage to see if it contains
   a data item that matches in some way another data item that you have.
   (for example, searching the closed set to see if a similar state is
   already there) The function "search_for" searches through the priority 
   queue looking for a data item similar to one you provide as a pattern. If
   one is found, it returns a pointer to that item; if none are found
   it returns NULL. To use search_for, you must define a function that
   tests two items to see if they are similar enough; the search_for
   function is called with 3 parameters: the priority queue, your is_similar
   function, and the item to be used as a pattern in the search. The
   is_similar function below illustrates how such a function could be
   written. For a use, see the example at the end. */

int is_similar(item *a, item *b)      /* remove or replace this */
{ int diff=a->value-b->value;
  if (diff<0) diff=-diff;
  if (diff<=1)
    return 1;
  else
    return 0; }

/*
   This kind of queue is only suitable for storing structured data (defined
   using a "struct" definition in C) like the "example_data_item" above.
   Data items in the queue are referred to through pointers.

   This implementation uses dynamic memory for all storage purposes. This
   means that there is no preset limit to the size of the queue, but it is
   less efficient in terms of execution time and memory use. If you have a good
   estimate of the maximum size the queue will grow to, you may be better off
   using an apriorityqueue.

   to create a dpriorityqueue, call "new_dpriorityqueue" with no parameters;
   store the returned result in a variable of type "dpriorityqueue *",
   and use it to refer to the queue in all further calls. If you wish to set a maximum
   size, use the function "set_dpriorityqueue_maximum_size".

   the only things you can do to a priorityqueue are add an item, remove the item
   that currently has the lowest priority, and find the total number of items held.
   All three functions refer to the set and the item through pointers.

   There is a small example below.

   When you have completely finished with a dpriorityqueue, you should call
   "delete_dpriorityqueue" to return its memory to the system, unless the whole
   program is about to terminate. */

#define theshift 8
#define maxind (1<<theshift)
#define mask (maxind-1)

typedef item *level0[maxind];
typedef level0 *level1[maxind];
typedef level1 *level2[maxind];

typedef struct
{ int size, max;
  level2 data; } dpriorityqueue;

dpriorityqueue *new_dpriorityqueue(void)
{ int i;
  dpriorityqueue *a=(dpriorityqueue *)malloc(sizeof(dpriorityqueue));
  a->size=0;
  a->max=-1;
  for (i=0; i<maxind; i+=1)
    a->data[i]=NULL;
  return (a); }

void set_dpriorityqueue_maximum_size(dpriorityqueue *a, int max)
{ a->max=max; }

void delete_dpriorityqueue(dpriorityqueue *a)
{ int i, j;
  for (i=0; i<maxind; i+=1)
  { if (a->data[i]!=NULL)
    { level1 *l1=a->data[i];
      for (j=0; j<maxind; j+=1)
      { if ((*l1)[j]!=NULL)
        { level0 *l0=(*l1)[j];
          free((void *)l0); } }
      free ((void *)l1); } }
  free((void *)a); }

item **locatedpq(dpriorityqueue *a, int n)
{ int n1, n2, n3, i;
  level1 *l1;
  level0 *l0;
  n3=n&mask;
  n>>=theshift;
  n2=n&mask;
  n>>=theshift;
  n1=n&mask;
  if (a->data[n1]==NULL)
  { l1=(level1 *)malloc(sizeof(level1));
    for (i=0; i<maxind; i+=1)
      (*l1)[i]=NULL;
    a->data[n1]=l1; }
  else
    l1=a->data[n1];
  if ((*l1)[n1]==NULL)
  { l0=(level0 *)malloc(sizeof(level0));
    for (i=0; i<maxind; i+=1)
      (*l0)[i]=NULL;
    (*l1)[n2]=l0; }
  else
    l0=(*l1)[n2];
  return &((*l0)[n3]); }

int is_dpriorityqueue_full(dpriorityqueue *a)
{ return (a->max>0 && a->size>=a->max); }

int is_dpriorityqueue_empty(dpriorityqueue *a)
{ return (a->size<=0); }

int items_in_dpriorityqueue(dpriorityqueue *a)
{ return (a->size); }

void add_to_dpriorityqueue(dpriorityqueue *a, item *i)
{ int current;
  if (a->max>0 && a->size>=a->max)
  { fprintf(stderr,"add_to_dpriorityqueue() when full\n");
    exit(1); }
  *locatedpq(a,a->size)=i;
  current=a->size;
  a->size+=1;
  while (current>0)
  { item *currentitem, *parentitem;
    int parent;
    parent=(current-1)/2;
    currentitem=*locatedpq(a,current);
    parentitem=*locatedpq(a,parent);
    if (priority_of(currentitem)>=priority_of(parentitem))
      break;
    *locatedpq(a,current)=parentitem;
    *locatedpq(a,parent)=currentitem;
    current=parent; } }

item *remove_from_dpriorityqueue(dpriorityqueue *a)
{ item *it;
  int current, left;
  if (a->size<=0)
  { fprintf(stderr,"remove_from_dpriorityqueue() when empty\n");
    exit(1); }
  it=*locatedpq(a,0);
  a->size-=1;
  *locatedpq(a,0)=*locatedpq(a,a->size);
  current=0;
  left=2*current+1;
  while (left<a->size)
  { int right, smallest;
    item *currentitem, *childitem, *smallestitem;
    right=left+1;
    currentitem=*locatedpq(a,current);
    smallest=current;
    smallestitem=currentitem;
    childitem=*locatedpq(a,left);
    if (priority_of(childitem)<priority_of(smallestitem))
    { smallestitem=childitem;
      smallest=left; }
    if (right<a->size)
    { childitem=*locatedpq(a,right);
      if (priority_of(childitem)<priority_of(smallestitem))
      { smallestitem=childitem;
        smallest=right; } }
    if (smallest==current)
      break;
    *locatedpq(a,current)=smallestitem;
    *locatedpq(a,smallest)=currentitem;
    current=smallest;
    left=2*current+1; }
  return (it); }

typedef int comparer(item *, item *);

item *search_dpriorityqueue_for(dpriorityqueue *a, comparer *fn, item *pattern)
{ int i,j,k;
  for (i=0; i<maxind; i+=1)
  { if (a->data[i]!=NULL)
    { level1 *l1=a->data[i];
      for (j=0; j<maxind; j+=1)
      { if ((*l1)[j]!=NULL)
        { level0 *l0=(*l1)[j];
          for (k=0; k<maxind; k+=1)
          { if ((*l0)[k]!=NULL)
            { item *it=(*l0)[k];
              if (fn(it,pattern))
                return (it); } } } } } }
  return NULL; }

/* the following "main" function is just to illustrate the use of an dpriorityqueue.
   It should not be included in any useful programs. */

void main(void)
{ dpriorityqueue *as=new_dpriorityqueue();
  example_data_item *x, *y;

  x=create_item(17);
  printf("adding %d to priorityqueue\n", x->value);
  add_to_dpriorityqueue(as,x);
  x=create_item(5);
  printf("adding %d to priorityqueue\n", x->value);
  add_to_dpriorityqueue(as,x);
  x=create_item(6666666);
  printf("adding %d to priorityqueue\n", x->value);
  add_to_dpriorityqueue(as,x);
  x=create_item(1);
  printf("adding %d to priorityqueue\n", x->value);
  add_to_dpriorityqueue(as,x);
  x=create_item(54321);
  printf("adding %d to priorityqueue\n", x->value);
  add_to_dpriorityqueue(as,x);
  x=create_item(8274);
  printf("adding %d to priorityqueue\n", x->value);
  add_to_dpriorityqueue(as,x);

  y=create_item(6);
  printf("\nsearching for something similar to %d\n", y->value);
  x=search_dpriorityqueue_for(as,is_similar,y);
  if (x==NULL)
    printf("  Nothing similar found\n\n");
  else
    printf("  Found %d, which is close enough\n\n", x->value);

  while (!is_dpriorityqueue_empty(as))
  { printf("removing an item from the priorityqueue ");
    x=remove_from_dpriorityqueue(as);
    printf("it was %d\n", x->value); }
  printf("The priorityqueue is now empty.\n");

  delete_dpriorityqueue(as); }