/*
   File:        pheap.c
   Author(s):   Andrew Moore and Ting Liu
   Created:     Wed Nov 13 08:17:59 EST 2002
   Description: New Faster Heap of Pointers
   Copyright (c) Carnegie Mellon University
*/

#include "pheap.h"

ph_node *mk_ph_node(void *data,double priority)
{
  ph_node *pn = AM_MALLOC(ph_node);
  pn->data = data;
  pn->priority = priority;
  return pn;
}

void free_ph_node(ph_node *x)
{
  AM_FREE(x,ph_node);
}

void fprintf_ph_node(FILE *s,char *m1,ph_node *x,char *m2)
{
  char *buff;

  buff = mk_printf("%s -> data",m1);
  fprintf(s,"%s = 0x%p%s",buff,(void *) x->data,m2);
  free_string(buff);

  buff = mk_printf("%s -> priority",m1);
  fprintf_double(s,buff,x->priority,m2);
  free_string(buff);
}

void pph_node(ph_node *x)
{
  fprintf_ph_node(stdout,"ph_node",x,"\n");
}

ph_node *mk_copy_ph_node(ph_node *old)
{
  ph_node *nu = AM_MALLOC(ph_node);

  nu -> data = old -> data;
  nu -> priority = old -> priority;

  return nu;
}
/* Makes a pheap (Simple HEAP) structure with initially nothing
   in the queue */
pheap *mk_pheap()
{
  pheap *x = AM_MALLOC(pheap);
  int i;

  x -> size = 0;
  x -> array_size = 10;
  x -> array = AM_MALLOC_ARRAY(ph_node *,x->array_size);
  for ( i = 0 ; i < x->array_size ; i++ )
    x->array[i] = NULL;

  return x;
}

void fprintf_pheap(FILE *s,char *m1,pheap *x,char *m2)
{
  char *buff;
  int i;

  buff = mk_printf("%s -> size",m1);
  fprintf_int(s,buff,x->size,m2);
  free_string(buff);

  buff = mk_printf("%s -> array_size",m1);
  fprintf_int(s,buff,x->array_size,m2);
  free_string(buff);

  for ( i = 0 ; i < x->size ; i++ )
  {
    buff = mk_printf("%s -> array[%d]",m1,i);
    fprintf_ph_node(s,buff,x->array[i],m2);
    free_string(buff);
  }
}

void ppheap(pheap *x)
{
  fprintf_pheap(stdout,"pheap",x,"\n");
}

pheap *mk_copy_pheap(pheap *old)
{
  pheap *nu = AM_MALLOC(pheap);
  int i;

  nu -> size = old -> size;
  nu -> array_size = old -> size;
  nu -> array = AM_MALLOC_ARRAY(ph_node *,old -> size);

  for ( i = 0 ; i < old -> size ; i++ )
    nu->array[i] = mk_copy_ph_node(old->array[i]);

  return nu;
}

/* children(hindex) = { 2 * hindex + 1 , 2 * hindex + 2 }

   parent(hindex) = (hindex-1)/2
*/


#define fast_internal_pheap_size(ph) ((ph)->size)
int slow_internal_pheap_size(pheap *ph)
{
  return fast_internal_pheap_size(ph);
}

#ifdef AMFAST
#define internal_pheap_size(ph) fast_internal_pheap_size(ph)
#else
#define internal_pheap_size(ph) slow_internal_pheap_size(ph)
#endif

#define fast_hindex_parent(hindex) ((hindex-1) >> 1)
int slow_hindex_parent(pheap *ph,int hindex)
{
  my_assert(hindex > 0 && hindex < internal_pheap_size(ph));
  return fast_hindex_parent(hindex);
}
#ifdef AMFAST
#define hindex_parent(ph,hindex) fast_hindex_parent(hindex)
#else
#define hindex_parent(ph,hindex) slow_hindex_parent(ph,hindex)
#endif

#define fast_hindex_child1(hindex) (((hindex) << 1) + 1)
int slow_hindex_child1(pheap *ph,int hindex)
{
  my_assert(hindex >= 0 && hindex < internal_pheap_size(ph));
  return fast_hindex_child1(hindex);
}
#ifdef AMFAST
#define hindex_child1(ph,hindex) fast_hindex_child1(hindex)
#else
#define hindex_child1(ph,hindex) slow_hindex_child1(ph,hindex)
#endif

#define fast_hindex_child2(hindex) (((hindex) << 1) + 2)
int slow_hindex_child2(pheap *ph,int hindex)
{
  my_assert(hindex >= 0 && hindex < internal_pheap_size(ph));
  return fast_hindex_child2(hindex);
}
#ifdef AMFAST
#define hindex_child2(ph,hindex) fast_hindex_child2(hindex)
#else
#define hindex_child2(ph,hindex) slow_hindex_child2(ph,hindex)
#endif

#define fast_pheap_hindex_to_ph_node(ph,hindex) ((ph)->array[hindex])
ph_node *slow_pheap_hindex_to_ph_node(pheap *ph,int hindex)
{
  ph_node *result;
  my_assert(hindex >= 0 && hindex <= internal_pheap_size(ph));
  result = fast_pheap_hindex_to_ph_node(ph,hindex);
  if ( result == NULL )
    my_error("pheap_hindex_to_ph_node: NULL ph_node");
  return result;
}
#ifdef AMFAST
#define pheap_hindex_to_ph_node(ph,hindex) fast_pheap_hindex_to_ph_node(ph,hindex)
#else
#define pheap_hindex_to_ph_node(ph,hindex) slow_pheap_hindex_to_ph_node(ph,hindex)
#endif

#define fast_pheap_hindex_to_priority(ph,hindex) (pheap_hindex_to_ph_node(ph,hindex)->priority)
double slow_pheap_hindex_to_priority(pheap *ph,int hindex)
{
  my_assert(hindex >= 0 && hindex <= internal_pheap_size(ph));
  return fast_pheap_hindex_to_priority(ph,hindex);
}
#ifdef AMFAST
#define pheap_hindex_to_priority(ph,hindex) fast_pheap_hindex_to_priority(ph,hindex)
#else
#define pheap_hindex_to_priority(ph,hindex) slow_pheap_hindex_to_priority(ph,hindex)
#endif

#define fast_pheap_hindex_to_data(ph,hindex) (pheap_hindex_to_ph_node(ph,hindex)->data)
ph_node *slow_pheap_hindex_to_data(pheap *ph,int hindex)
{
  my_assert(hindex >= 0 && hindex <= internal_pheap_size(ph));
  return fast_pheap_hindex_to_data(ph,hindex);
}
#ifdef AMFAST
#define pheap_hindex_to_data(ph,hindex) fast_pheap_hindex_to_data(ph,hindex)
#else
#define pheap_hindex_to_data(ph,hindex) slow_pheap_hindex_to_data(ph,hindex)
#endif

int ph_node_code(ph_node *pn)
{
  return (int) pn;
}

int hindex_to_code(pheap *ph,int hindex)
{
  return ph_node_code(pheap_hindex_to_ph_node(ph,hindex));
}

ivec *mk_hindex_to_code(pheap *ph)
{
  ivec *h2c = mk_ivec(internal_pheap_size(ph));
  int i;
  for ( i = 0 ; i < internal_pheap_size(ph) ; i++ )
    ivec_set(h2c,i,hindex_to_code(ph,i));
  return h2c;
}

void check_pheap(pheap *ph)
{
  int i;
  ivec *h2c = mk_hindex_to_code(ph);
  ivec *siv = mk_sivec_from_ivec(h2c);

  if ( ivec_size(siv) < ivec_size(h2c) )
    my_errorf("pheap array contains duplicates");

  for ( i = 0 ; i < internal_pheap_size(ph) ; i++ )
  {
    int child1 = hindex_child1(ph,i);
    int child2 = hindex_child2(ph,i);

    if ( child1 < internal_pheap_size(ph) )
      my_assert(pheap_hindex_to_priority(ph,i) <= 
		pheap_hindex_to_priority(ph,child1));

    if ( child2 < internal_pheap_size(ph) )
      my_assert(pheap_hindex_to_priority(ph,i) <= 
		pheap_hindex_to_priority(ph,child2));
  }

  free_ivec(h2c);
  free_ivec(siv);
}

/* 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 pheap, or until
   "data" has been removed from the pheap. */
void add_to_pheap(pheap *ph,void *data,double priority)
{
  int hindex = internal_pheap_size(ph);
  bool finished = FALSE;
  ph_node *pn = mk_ph_node(data,priority);
#ifdef DOING_ASSERTS
  int new_code = ph_node_code(pn);
#endif

  if ( ph->array_size == hindex )
  {
    int new_size = 2 * ph->array_size + 2;
    int i;
    ph_node **new_array = AM_MALLOC_ARRAY(ph_node *,new_size);
    for ( i = 0 ; i < hindex ; i++ )
      new_array[i] = pheap_hindex_to_ph_node(ph,i);
    for ( i = hindex ; i < new_size ; i++ )
      new_array[i] = NULL;
    AM_FREE_ARRAY(ph->array,ph_node *,ph->array_size);
    ph->array = new_array;
    ph->array_size = new_size;
  }

  ph->size += 1;
  ph->array[hindex] = pn;

  while ( !finished )
  {
    if ( hindex == 0 )
      finished = TRUE;
    else
    {
      int parent = hindex_parent(ph,hindex);
      if ( pheap_hindex_to_priority(ph,parent) > priority )
      {
	ph->array[hindex] = pheap_hindex_to_ph_node(ph,parent);
	ph->array[parent] = pn;
	my_assert(ph->array[hindex] != ph->array[parent]);
	hindex = parent;
      }
      else
	finished = TRUE;
    }
  }
#ifndef AMFAST
  {
    ivec *h2c = mk_hindex_to_code(ph);
    check_pheap(ph);
    my_assert(is_in_ivec(h2c,new_code));
    free_ivec(h2c);
  }
#endif
}

/* 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 pheap_minimum_priority(pheap *ph)
{
#ifndef AMFAST
  if ( internal_pheap_size(ph) == 0 ) my_error("pheap_minimum_priority: pheap empty");
#endif
  return pheap_hindex_to_priority(ph,0);
}


/* 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 *pheap_data_with_minimum_priority(pheap *ph)
{
#ifndef AMFAST
  if ( internal_pheap_size(ph) == 0 ) my_error("pheap_data_with_minimum_priority: "
				      "pheap empty");
#endif
  return pheap_hindex_to_data(ph,0);
}

int pheap_size(pheap *ph)
{
  return ph->size;
}

/* do linear search, find the index of node with priority */
int pheap_linear_search(pheap *ph, void *data)
{
  int i;
  int size = internal_pheap_size(ph);

  /*  printf("priority = %f\n",priority);*/

  for( i=0; i<size; i++ )
    {
      /* printf("search priority=%f\n",ph->array[i]->priority);*/
      if ( ph->array[i]->data == data)
	  return i;
    }
  return -1;
}
       
/* This function delete the element from pheap with index hindex */
void pheap_remove_element(pheap *ph, int hindex)
{
  bool finished = FALSE;
  /*
#ifdef DOING_ASSERTS
  int lowest_code = hindex_to_code(ph,hindex);
#endif
  */

#ifdef DOING_ASSERTS
  if ( internal_pheap_size(ph) == hindex ) my_error("pheap_remove_minimum_priority: "
				      "pheap size is wrong");
#endif

  free_ph_node(pheap_hindex_to_ph_node(ph,hindex));


  ph->array[hindex] = pheap_hindex_to_ph_node(ph,internal_pheap_size(ph)-1);
#ifndef AMFAST
  ph->array[internal_pheap_size(ph)-1] = NULL;
#endif

  ph->size -= 1;

  while ( !finished )
  {
    int child1 = fast_hindex_child1(hindex);
    if ( child1 >= internal_pheap_size(ph) )
      finished = TRUE;
    else 
    {
      ph_node *pn_me = pheap_hindex_to_ph_node(ph,hindex);
      ph_node *pn1 = pheap_hindex_to_ph_node(ph,child1);
      int child2 = hindex_child2(ph,hindex);
      double pri_me = pn_me -> priority;
      double pri1 = pn1 -> priority;

      my_assert(child2 <= internal_pheap_size(ph));

      if ( child2 == internal_pheap_size(ph) )
      {
	if ( pri_me > pri1 )
	{
	  ph->array[hindex] = pn1;
	  ph->array[child1] = pn_me;
	}
	finished = TRUE;
      }
      else
      {
	ph_node *pn2 = pheap_hindex_to_ph_node(ph,child2);
	double pri2 = pn2 -> priority;
	
	if ( pri1 < pri2 )
	{
	  if ( pri_me <= pri1 )
	    finished = TRUE;
	  else
	  {
	    ph->array[hindex] = pn1;
	    ph->array[child1] = pn_me;
	    hindex = child1;
	  }
	}
	else
	{
	  if ( pri_me <= pri2 )
	    finished = TRUE;
	  else
	  {
	    ph->array[hindex] = pn2;
	    ph->array[child2] = pn_me;
	    hindex = child2;
	  }
	}
      }
    }
  }
}

/* This deletes the lowest priority item. The data memory is not freed. */
void pheap_remove_minimum_priority(pheap *ph)
{
  pheap_remove_element(ph,0);
}


/* Frees the pheap 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
   responsibiliy to eventually clean up the memory for that data item. */
void free_pheap(pheap *ph)
{
  int i;
  for ( i = 0 ; i < internal_pheap_size(ph) ; i++ )
    free_ph_node(pheap_hindex_to_ph_node(ph,i));
  AM_FREE_ARRAY(ph->array,ph_node *,ph->array_size);
  AM_FREE(ph,pheap);
}

void pheap_main(int argc,char *argv[])
{
  dyv *pris = mk_dyv_from_string("3 1 4 1 5 9 2 6 5 3 5 8 9 7 9 3 2 3 8 4 6 2 "
				 "6 4 3 3 8 3 1 4 1 2 2 6 8 7 5 9 9 6 5 0 2",
				 NULL);
  pheap *ph = mk_pheap();
  int i;

  for ( i = 0 ; i < dyv_size(pris) ; i++ )
  {
    double pri_on = dyv_ref(pris,i);
    add_to_pheap(ph,NULL,pri_on);
    printf("Added priority %g to pheap, which now has lowest priority %g\n",
	   pri_on,pheap_minimum_priority(ph));
    wait_for_key();
    if ( i % 2 == 1 )
    {
      pheap_remove_minimum_priority(ph);
      printf("Removed minimum priority item\n");
      wait_for_key();
    }
    ppheap(ph);
  }

  free_pheap(ph);
  free_dyv(pris);
}
