#include <stdlib.h>
#include <string.h>

#include "gheap.h"
#include "ruby.h"

#ifdef GHEAP_COMPILE
typedef struct
{
   int i;
   int w;
} gheap_node_t ;


struct gheap
{
   int              max; 
   int              size;

   int             *perm1;   
   int             *perm2;

   gheap_node_t    *arr;
};

static void *reall ( void          *source,  
                     int           newSize,
                     int           oldSize)
{
   void   *target;
   
   if ( !(target = malloc( newSize)))
      return NULL;
   memcpy(target, source, oldSize);  
   free( source );
   
   return target;
}

void gheap_reset ( gheap_t   h )
{
   int i;
   h->max = 0;
}


gheap_t gheap_new ( int          size)
{
   gheap_t h;
   int     i; 
   
   if (size <= 0) {
      rb_fatal("gheap_init [E]: size of heap <= 0");      return NULL;
   }
   
   /* create a heap with 'size' elements and initialize it */
   if ( !(h = (gheap_t) malloc( sizeof *h)) )
      return NULL;
   
   /* create the arr of size 'size'+1 (Element 0 is not used) */
   h->size = size+1;
   if ( !(h->arr = (gheap_node_t *) 
          malloc( h->size * sizeof(gheap_node_t))) ) {
      free( h);
      return NULL;
   }
   if ( !(h->perm1 = (int *) malloc( h->size * sizeof(int))) ) {
      free( h );
      free( h->arr );
      return NULL;
   }
   if ( !(h->perm2 = (int *) malloc( h->size * sizeof(int))) ) {
      free( h);
      free( h->arr );
      free( h->perm1 );
      return NULL;
   }
   for (i=0;i<h->size;i++) {
      h->perm1[i]   = i;
      h->perm2[i] = i;
    }
   gheap_reset(h);
   
   return h;
}

void gheap_clear ( gheap_t h )
{
   free( h->arr );
   free( h->perm1 );
   free( h->perm2);
   free( h );
}

static int uph ( gheap_t           h,
                 gheap_node_t     *arr,
                 int               k,
                 int              *perm1,
                 int              *perm2)
{
   int   v   = perm1[k];
   gheap_node_t *obj = &(arr[v]);
   int   rc  = 0;
   
   while (k != 1 && (arr[perm1[k/2]].w-obj->w) > 0) {
      perm1[k] = perm1[k/2];
      perm2[perm1[k]] = k;
      rc = 1;
      k = k/2;
   }
   perm1[k] = v;
   perm2[v] = k;
   return rc; 
}

int gheap_add ( gheap_t       h,
                int           i,
                int           w)
{    
   int rc;
   
   (h->max)++;
   if (h->max >= h->size) {
      gheap_node_t *arr;
      int            *perm1, *perm2, i;
      
      if ( !(arr = (gheap_node_t *) reall(h->arr, 2 * h->size * sizeof(gheap_node_t),
                                          h->size * sizeof(gheap_node_t))))
         return -1;
      h->arr = arr;
      if ( !(perm1 = (int *) reall(h->perm1, 2 * h->size * sizeof(int),
                                   h->size * sizeof(int))))
         return -1;
      h->perm1 = perm1;
      if ( !(perm2 = (int *) reall(h->perm2, 2 * h->size * sizeof(int),
                                   h->size * sizeof(int))))
         return -1;
      h->perm2 = perm2;
      for (i=h->size;i<h->size*2;i++) {
         h->perm1[i] = i;
         h->perm2[i] = i;
      }
      h->size *= 2;
   }
   h->arr[h->perm1[h->max]].i = i;
   h->arr[h->perm1[h->max]].w = w;
   rc = h->perm1[h->max];
   uph(h,h->arr, h->max, h->perm1, h->perm2);
   
   return rc;
}

static int downh ( gheap_t       h,
                   gheap_node_t *arr,
                   int          k,
                   int          max,
                   int         *perm1,
                   int         *perm2)
{
   gheap_node_t* obj;
   int    max_half, j, v, rc=0;
   
   max_half = max / 2;
   v = perm1[k];
   obj = &(arr[v]);
   while (k <= max_half) {
      j = k+k;
      if (j < max && ((arr[perm1[j]].w - arr[perm1[j+1]].w) > 0))
         j++;
      
      if ((obj->w - arr[perm1[j]].w) <= 0)
         break;
      
      perm1[k] = perm1[j];
      perm2[perm1[k]] = k;
        rc = 1;
        k = j;
   }
   
   perm1[k] = v; 
   perm2[v] = k;
   return rc;
}

int gheap_get ( gheap_t   h,
                int * w)
{
   gheap_node_t *obj;
   int tmp;
   
   if (gheap_is_empty(h) )
      return -1;
   
   tmp = h->perm1[1];
   obj = &(h->arr[tmp]);
   h->perm1[1] = h->perm1[h->max];
   h->perm1[h->max] = tmp;
   h->perm2[h->perm1[1]] = 1;
   h->perm2[h->perm1[h->max]] = h->max;
   
   (h->max)--;
   
   downh(h,h->arr, 1, h->max, h->perm1, h->perm2);

   *w = obj->w;
   return obj->i;
} 

int gheap_remove ( gheap_t     h,
                   int         n )
{    
   gheap_node_t *obj;
   int   els = h->perm2[n];
   int   tmp;

   if (els<h->max) {
      tmp = h->perm1[els];
/*      obj = &(h->arr[tmp]); */
      h->perm1[els] = h->perm1[h->max];
      h->perm1[h->max] = tmp;
      h->perm2[h->perm1[els]] = els;
      h->perm2[h->perm1[h->max]] = h->max;
      
      (h->max)--;
      
      if (!downh(h,h->arr, els, h->max, h->perm1, h->perm2)) 
         uph(h,h->arr, els, h->perm1, h->perm2);
   }
   else {
      if (els==h->max) {
         (h->max)--;
      }
      else {
         rb_fatal("Deletion of element %i is not in the heap !",n);
      }
   }
   return 0;
}

int gheap_relabel ( gheap_t     h,
                    int         n ,
                    int         w )
{    
   int els = h->perm2[n];
   h->arr[h->perm1[n]].w = w;
   if (els>h->max) {
      rb_fatal("Relabeling of element %i not in the heap !",n);
   }
   
   if (!uph(h,h->arr,els,h->perm1,h->perm2) ) {
      downh(h,h->arr,els,h->max,h->perm1,h->perm2);
   }
   return 0;
}

int gheap_weight(gheap_t h,
                 int n)
{
   return h->arr[h->perm1[n]].w;
}

int gheap_is_empty ( gheap_t  h )
{
   return (h->max == 0);
} 

#endif
