/*
     File:         heap.c
     Author:       Andrew W. Moore
     Created:      14th February 1992
     Description:  The heap for A-Star search.
     Modified:     Jan 2001, to create the more modern (but slightly less
                   powerful) sheap API.
*/

#include <stdio.h>
#include <math.h>
#include "sheap.h"

void sift_down ( heap *hp, void *x )
{
  int k = hp->heap_index_of(x);
  bool finished = FALSE;

  while ( !finished )
  {
    int leftj = 1 + 2 * k; /* Notice our arrays are indexed 0..heapsize-1 */
    int rightj = 2 + 2 * k;

    if ( leftj >= hp -> size )
      finished = TRUE;
    else
    {
      int next_k;
      if ( rightj < hp->size )
        next_k = ( hp->is_better_than(hp->data[leftj],hp->data[rightj]) ) ?
                 leftj :
                 rightj;
      else
        next_k = leftj;

      if ( hp->is_better_than(hp->data[next_k],x) )
      {
        hp->data[k] = hp->data[next_k];
        hp->set_heap_index(hp->data[k],k);
        k = next_k;
      }
      else
        finished = TRUE;
    }
  }

  hp->data[k] = x;
  hp->set_heap_index(hp->data[k],k);
}

void percolate ( heap *hp, void *x )
{
  int k = hp->heap_index_of(x);
  bool finished = FALSE;

  while ( k > 0 && !finished )
  {
    int parent_k = (k-1)/2;
    if ( hp->is_better_than(hp->data[parent_k],x) )
      finished = TRUE;
    else
    {
      hp->data[k] = hp->data[parent_k];
      hp->set_heap_index(hp->data[k],k);
      k = parent_k;
    }
  }

  hp->data[k] = x;
  hp->set_heap_index(hp->data[k],k);
}

void *find_and_remove_best ( heap *hp )
{
  void *result;

  if ( hp == NULL )
    my_error("find_and_remove_best: heap is NULL\n");

  if ( Verbosity > 100.0 )
    printf("Removing something from a heap of size %d\n",hp->size);
  if ( hp->size <= 0 )
    result = NULL;
  else
  {
    result = hp->data[0];
    hp->set_heap_index(result,-1);

    if ( hp->size == 1 )
      hp->size = 0;
    else
    {
      void *newtop = hp->data[hp->size-1];

      hp->data[0] = newtop;
      hp->set_heap_index(newtop,0);

      hp->data[hp->size-1] = NULL;
      hp->size -= 1;

      sift_down(hp,newtop);
    }
  }

  return(result);
}

void note_changed_value ( heap *hp, void *x )
{
  int index;

  if ( hp == NULL )
    my_error("note changed value: heap is NULL\n");

  if ( Verbosity > 100.0 )
    printf("Noting changed value on a heap of size %d\n",hp->size);

  index = hp->heap_index_of(x);
  if ( index < 0 || index >= hp -> size )
    my_error("Note changed value: crap heap index\n");
  
  if ( index > 0 && hp->is_better_than(x,hp->data[(index-1)/2]) )
    percolate(hp,x);
  else
    sift_down(hp,x);
}

void add_to_heap ( heap *hp, void *new_x )
{
  if ( hp == NULL )
    my_error("add_to_heap: heap is NULL\n");

  if ( Verbosity > 100.0 )
   printf("Adding to a heap of size %d (max_size %d)\n",hp->size,hp->max_size);

  if ( hp->size >= hp->max_size )
    my_error("heapsize too large to insert new thing");
  else
  {
    hp->data[hp->size] = new_x;
    hp->set_heap_index(new_x,hp->size);
    hp->size += 1;
   
    percolate(hp,new_x);
  }
}

typedef void *charptr;

void get_heap_ready ( heap *hp )
/* 
   PRE: heap must not have already been got ready.
   The following fields should have been set up to taste:

    hp -> max_size
    hp -> is_better_than
    hp -> heap_index_of
    hp -> set_heap_index

   POST: You're ready to run with the three function described above
*/
{
  hp -> data = AM_MALLOC_ARRAY(charptr,hp->max_size);
  hp -> size = 0;
}

int heap_size ( heap *hp )
{
  return(hp->size);
}

void pq_empty ( pqueue *pq )
/* Empty a priority queue */
{
  int i;
  for ( i = 0 ; i < pq->heap.max_size ; i++ )
    pq->pqelts[i]->heap_index = -1;

  pq->heap.size = 0;
}

bool pq_is_better_than ( void *hi1, void *hi2 )
{
  return( ((pqelt *)hi1)->priority > ((pqelt *)hi2)->priority );
}

int pq_heap_index_of ( void *hi )
{
  return( ((pqelt *)hi)->heap_index );
}

void pq_set_heap_index ( void *hi, int val )
{
  ((pqelt *)hi)->heap_index = val;
}


pqueue *pq_create ( int max_input )
/* all inputs you give to pqueue must be between 0 .. max_input-1 inclusive*/
{
  int i;
  pqueue *pq = AM_MALLOC(pqueue);
  pq->heap.max_size = max_input;
  pq->heap.is_better_than = pq_is_better_than;
  pq->heap.heap_index_of = pq_heap_index_of;
  pq->heap.set_heap_index = pq_set_heap_index;
  
  get_heap_ready(&pq->heap);

  pq->pqelts = AM_MALLOC_ARRAY(pqelt_ptr,max_input);

  for ( i = 0 ; i < max_input ; i++ )
  {
    pq->pqelts[i] = AM_MALLOC(pqelt);
    pq->pqelts[i]->input = i;
  }
  pq_empty(pq);
  return(pq);
}

void free_pqueue(pqueue *pq)
{
  int i;
  AM_FREE_ARRAY(pq->heap.data,char_ptr,pq->heap.max_size);
  for ( i = 0 ; i < pq->heap.max_size ; i++ )
    AM_FREE(pq->pqelts[i],pqelt);

  AM_FREE_ARRAY(pq->pqelts,pqelt_ptr,pq->heap.max_size);
  AM_FREE(pq,pqueue);
}

bool pq_is_empty ( pqueue *pq )
{
  return(pq->heap.size == 0);
}

int pq_size ( pqueue *pq )
{
  return(pq->heap.size);
}

int pq_pop_best ( pqueue *pq )
{
  int result;

  if ( pq->heap.size == 0 )
  {
    result = 0;
    my_error("Trying to pop empty pri queue");
  }
  else
  {
    pqelt *pe = (pqelt *) find_and_remove_best(&pq->heap);
    result = pe->input;
  }

  return(result);
}

void pq_insert ( pqueue *pq, int i, double pri )
{
  if ( i < 0 || i >= pq->heap.max_size )
    my_error("sobidcal");
  else
  {
    pqelt *pe = pq->pqelts[i];
    pe -> priority = pri;

    if ( pe -> heap_index < 0 )
      add_to_heap(&pq->heap,(void *) pe);
    else
      note_changed_value(&pq->heap,(void *)pe);
  }
}

bool pq_contains ( pqueue *pq, int input )
{
  return(pq->pqelts[input]->heap_index >= 0);
}
    
double pq_priority ( pqueue *pq, int input )
{
  if (pq->pqelts[input]->heap_index < 0)
    my_error("heap.c : pq_priority: asked for priority of thing not on queue");

  return(pq->pqelts[input]->priority);
}

double priority_of_best ( pqueue *pq )
{
  double result;

  if ( pq->heap.size < 1 )
  {
    result = 0.0;
    my_error("iusvcdbuvcbka");
  }
  else
  {
    void *top = pq->heap.data[0];
    pqelt *pqe = (pqelt *) top;
    result = pqe->priority;
  }

  return(result);
}

/***************** SHEAP *******************/

typedef struct sheap_elt
{
  void *data;
  double priority;
  int heap_index;
} sheap_elt;

bool sheap_is_better_than(void *data1,void *data2)
{
  sheap_elt *se1 = (sheap_elt *) data1;
  sheap_elt *se2 = (sheap_elt *) data2;
  return se1->priority < se2->priority;
}

int sheap_heap_index_of(void *data)
{
  sheap_elt *se = (sheap_elt *) data;
  return se->heap_index;
}

void sheap_set_heap_index(void *data,int heap_index)
{
  sheap_elt *se = (sheap_elt *) data;
  se->heap_index = heap_index;
}

heap *mk_sheap_heap(int max_size)
{
  heap *hp = AM_MALLOC(heap);
  hp -> max_size = max_size;
  hp -> is_better_than = sheap_is_better_than;
  hp -> heap_index_of = sheap_heap_index_of;
  hp -> set_heap_index = sheap_set_heap_index;
  hp -> data = NULL;
  hp -> size = -1;
  get_heap_ready(hp);
  return hp;
}

/* Makes a sheap (Simple HEAP) structure with initially nothing
   in the queue */
sheap *mk_sheap(void)
{
  sheap *sh = AM_MALLOC(sheap);
  sh -> hp = mk_sheap_heap(3);
  return sh;
}

sheap *mk_sheap_given_max_size(int max_size)
{
  sheap* sh = AM_MALLOC(sheap);
  sh -> hp = mk_sheap_heap(max_size);
  return sh;
}

void free_heap_shallow(heap *hp)
{
  AM_FREE_ARRAY(hp->data,void *,hp->max_size);
  AM_FREE(hp,heap);
}

/* Adds the given pointer to the heap with the given priority.
   Notice that it directly stores the pointer...it does not
   make a copy of the thing in "data". This means that the caller
   must not deallocate (or otherwise hurt) whatever it is that "data"
   points to until after we're done with using the sheap, or until
   "data" has been removed from the sheap. */
void add_to_sheap(sheap *sh,void *data,double priority)
{
  sheap_elt *se = AM_MALLOC(sheap_elt);

  se -> priority = priority;
  se -> data = data;
  se -> heap_index = -77;

  if ( sh->hp->size >= sh->hp->max_size )
  {
    heap *new_hp = mk_sheap_heap(2 * sh->hp->max_size);
    int i;
    for ( i = 0 ; i < sh->hp->max_size ; i++ )
      add_to_heap(new_hp,sh->hp->data[i]);
    free_heap_shallow(sh->hp);
    sh->hp = new_hp;
  }
  add_to_heap(sh->hp,(void *) se);
}

/* The number of items on the sheap */
int sheap_size(sheap *sh)
{
  return sh->hp->size;
}

/* The following three operations all refer to the "next_to_pop"
   item, which is the item with the smallest value of "priority". Ties are
   broken by some arbitrary rule that may depend on order of insertion. */

/* The priority of the item "next_to_pop" item on the heap (i.e. the
   item that has minimum priority). Note that this "next_to_pop" item is
   NOT popped by this operation. */
double sheap_minimum_priority(sheap *sh)
{
  sheap_elt *se = (sheap_elt *) sh->hp->data[0];
  return se->priority;
}

/* The data pointer of the "next_to_pop" item on the heap (i.e. the
   item that has minimum priority). Note that this "next_to_pop" item is
   NOT popped by this operation. */
void *sheap_data_with_minimum_priority(sheap *sh)
{
  sheap_elt *se = (sheap_elt *) sh->hp->data[0];
  return se->data;
}

/* This deletes the lowest priority item. No memory is freed. */
void sheap_remove_minimum_priority(sheap *sh)
{
  sheap_elt *se = (sheap_elt *) find_and_remove_best(sh->hp);
  AM_FREE(se,sheap_elt);
}

/* Frees the sheap and all memory that it used, except does not
   do anything with the defunct data items that had been stored within it.
   Thus if a data item had been stored in the heap, it is the callers
   responsibility to eventually clean up the memory for that data item. */
void free_sheap(sheap *sh)
{
  while ( sheap_size(sh) > 0 )
    sheap_remove_minimum_priority(sh);
  free_heap_shallow(sh->hp);
  AM_FREE(sh,sheap);
}

void test_explain_sheap(sheap *sh)
{
  if ( sheap_size(sh) <= 0 )
    printf("sheap is empty\n");
  else
    printf("The minimum-priority thing on heap is \"%s\" with priority %g\n",
	   (char *) sheap_data_with_minimum_priority(sh),
	   sheap_minimum_priority(sh));
  wait_for_key();
}

void test_add_to_sheap(sheap *sh,char *s)
{
  double priority = (double) strlen(s);
  add_to_sheap(sh,s,priority);
  printf("Just added \"%s\" with priority %g\n",s,priority);
}

void test_sheap(void)
{
  sheap *sh = mk_sheap();
  string_array *sa = mk_broken_string(
    "sing in the town where I was born lived a man who sailed the seas");
  int i;

  test_explain_sheap(sh);
  for ( i = 0 ; i < string_array_size(sa) ; i++ )
  {
    test_add_to_sheap(sh,string_array_ref(sa,i));
    test_explain_sheap(sh);
    i++;
    if ( i < string_array_size(sa) )
    {
      test_add_to_sheap(sh,string_array_ref(sa,i));
      test_explain_sheap(sh);
    }
    sheap_remove_minimum_priority(sh);
    printf("Just removed minimum priority\n");
    test_explain_sheap(sh);

  }

  while ( sheap_size(sh) > 0 )
  {
    sheap_remove_minimum_priority(sh);
    printf("Just removed minimum priority\n");
    test_explain_sheap(sh);
  }

  free_string_array(sa);
  free_sheap(sh);
}

void sheap_main(int argc,void *argv[])
{
  test_sheap();
}

int sheap_max_size(sheap *sh)
{
  return sh->hp->max_size;
}

/***************** End SHEAP *******************/

/***************** Begin intheap *******************/

/* An intheap is the structure of choice if you wish to work with
   a set of integers (which we'll call a set of codes) in which
   each code has a priority. 

   So the structure represents a set of zero or more pairs

      (code0,priority0)
      (code1,priority1)
         :
      (code[N-1],priority[n-1])

   You can add any (code,priority) pair to this set in time log(n)

   You can find out which code has the lowest priority value in time O(1)

   You can find what that lowest priority is in time O(1)

   You can remove the (code,priority) pair with the lowest priority
   value in time O(log n)
*/

typedef struct intheap_elt
{
  int code;
  double priority;
  int heap_index;
} intheap_elt;

intheap_elt *mk_copy_intheap_elt( intheap_elt *ih_elt)
{
  intheap_elt *newih_elt;

  newih_elt = AM_MALLOC( intheap_elt);
  newih_elt->code       = ih_elt->code;
  newih_elt->priority   = ih_elt->priority;
  newih_elt->heap_index = ih_elt->heap_index;

  return newih_elt;
}

bool intheap_is_better_than(void *data1,void *data2)
{
  intheap_elt *ih_elt1 = (intheap_elt *) data1;
  intheap_elt *ih_elt2 = (intheap_elt *) data2;
  return ih_elt1->priority < ih_elt2->priority;
}

int intheap_heap_index_of(void *data)
{
  intheap_elt *ih_elt = (intheap_elt *) data;
  return ih_elt->heap_index;
}

void intheap_set_heap_index(void *data,int heap_index)
{
  intheap_elt *ih_elt = (intheap_elt *) data;
  ih_elt->heap_index = heap_index;
}

heap *mk_intheap_heap(int max_size)
{
  heap *hp = AM_MALLOC(heap);
  hp -> max_size = max_size;
  hp -> is_better_than = intheap_is_better_than;
  hp -> heap_index_of = intheap_heap_index_of;
  hp -> set_heap_index = intheap_set_heap_index;
  hp -> data = NULL;
  hp -> size = -1;
  get_heap_ready(hp);
  return hp;
}

/* Makes a intheap (Integer HEAP) structure with initially nothing
   in the queue */
intheap *mk_intheap(void)
{
  intheap *ih = AM_MALLOC(intheap);
  ih -> hp = mk_intheap_heap(3);
  return ih;
}

intheap *mk_intheap_given_max_size(int max_size)
{
  intheap *ih = AM_MALLOC(intheap);
  ih -> hp = mk_intheap_heap(max_size);
  return ih;
}

intheap *mk_copy_intheap( intheap *ih)
{
  int i;
  charptr data;
  heap *oldhp, *newhp;
  intheap *newih;
  intheap_elt *ih_elt;

  oldhp = ih->hp;

  /* Allcate space for new intheap. */
  newih = AM_MALLOC(intheap);
  newih->hp = AM_MALLOC( heap);
  newhp = newih->hp;

  /* Fill in easy stuff. */
  newhp->max_size       = oldhp->max_size;
  newhp->is_better_than = oldhp->is_better_than;
  newhp->heap_index_of  = oldhp->heap_index_of;
  newhp->set_heap_index = oldhp->set_heap_index;
  newhp->data           = AM_MALLOC_ARRAY( charptr, oldhp->max_size);
  newhp->size           = oldhp->size;

  /* Copy heap elements. */
  for (i=0; i < oldhp->size; ++i) {
    data = oldhp->data[i];
    if (data != NULL) {
      ih_elt = mk_copy_intheap_elt( (intheap_elt *) data);
    }
    else {
      ih_elt = NULL;
    }
    newhp->data[i] = (charptr) ih_elt;
  }

  /* Done. */
  return newih;
}

/* Adds the given pointer to the heap with the given priority.
   Notice that it directly stores the pointer...it does not
   make a copy of the thing in "data". This means that the caller
   must not deallocate (or otherwise hurt) whatever it is that "data"
   points to until after we're done with using the intheap, or until
   "data" has been removed from the intheap. */
void add_to_intheap(intheap *ih,int code,double priority)
{
  intheap_elt *ih_elt = AM_MALLOC(intheap_elt);

  ih_elt -> priority = priority;
  ih_elt -> code = code;
  ih_elt -> heap_index = -77;

  if ( ih->hp->size >= ih->hp->max_size )
  {
    heap *new_hp = mk_intheap_heap(2 * ih->hp->max_size);
    int i;
    for ( i = 0 ; i < ih->hp->max_size ; i++ )
      add_to_heap(new_hp,ih->hp->data[i]);
    free_heap_shallow(ih->hp);
    ih->hp = new_hp;
  }
  add_to_heap(ih->hp,(void *) ih_elt);
}

/* The number of items on the intheap */
int intheap_size(intheap *ih)
{
  return ih->hp->size;
}

/* The following three operations all refer to the "next_to_pop"
   item, which is the item with the smallest value of "priority". Ties are
   broken by some arbitrary rule that may depend on order of insertion. */

/* The priority of the item "next_to_pop" item on the heap (i.e. the
   item that has minimum priority). Note that this "next_to_pop" item is
   NOT popped by this operation. */
double intheap_minimum_priority(intheap *ih)
{
  intheap_elt *ih_elt = (intheap_elt *) ih->hp->data[0];
  return ih_elt->priority;
}

/* The code of the "next_to_pop" item on the heap (i.e. the
   code that has minimum priority). Note that this "next_to_pop" item is
   NOT popped by this operation. */
int intheap_code_with_minimum_priority(intheap *ih)
{
  intheap_elt *ih_elt = (intheap_elt *) ih->hp->data[0];
  return ih_elt->code;
}

/* This deletes the lowest priority item */
void intheap_remove_minimum_priority(intheap *ih)
{
  intheap_elt *ih_elt = (intheap_elt *) find_and_remove_best(ih->hp);
  AM_FREE(ih_elt,intheap_elt);
}

/* Frees the intheap and all memory that it used */
void free_intheap(intheap *ih)
{
  while ( intheap_size(ih) > 0 )
    intheap_remove_minimum_priority(ih);
  free_heap_shallow(ih->hp);
  AM_FREE(ih,intheap);
}

void test_explain_intheap(intheap *ih)
{
  if ( intheap_size(ih) <= 0 )
    printf("intheap is empty\n");
  else
    printf("The minimum-priority code on heap is %d with priority %g\n",
	   intheap_code_with_minimum_priority(ih),
	   intheap_minimum_priority(ih));
  wait_for_key();
}

void test_add_to_intheap(intheap *ih,int code)
{
  double priority = (double) ((code % 2==0) ? code : -code);
  add_to_intheap(ih,code,priority);
  printf("Just added code %d with priority %g\n",code,priority);
}

void test_intheap(void)
{
  intheap *ih = mk_intheap();
  ivec *iv = mk_ivec_from_string("3 1 4 1 5 9 2 6 5 3 5 8 9 7 9 3 2 3");
  int i;

  test_explain_intheap(ih);
  for ( i = 0 ; i < ivec_size(iv) ; i++ )
  {
    test_add_to_intheap(ih,ivec_ref(iv,i));
    test_explain_intheap(ih);
    i++;
    if ( i < ivec_size(iv) )
    {
      test_add_to_intheap(ih,ivec_ref(iv,i));
      test_explain_intheap(ih);
    }
    intheap_remove_minimum_priority(ih);
    printf("Just removed minimum priority\n");
    test_explain_intheap(ih);

  }

  while ( intheap_size(ih) > 0 )
  {
    intheap_remove_minimum_priority(ih);
    printf("Just removed minimum priority\n");
    test_explain_intheap(ih);
  }

  free_ivec(iv);
  free_intheap(ih);
}

void intheap_main(int argc,char *argv[])
{
  test_intheap();
}

int intheap_max_size(intheap *ih)
{
  return ih->hp->max_size;
}
  

