#include "heapsort.h"
#include <assert.h>

static inline void swap(char *a,char *b,size_t size) // {{{
{
  do { 
    char tmp=*a;
    *a++=*b;
    *b++=tmp;
  } while (--size);
}
// }}}

static void sink(void *base,size_t nmemb,size_t size,int (*compa)(const void *,const void *),int pos) // {{{
{
  int next=2*pos+1;
  while (next<nmemb) {
    const void *e0=(void *)(((const char *)base)+(pos*size)),
               *e1=(void *)(((const char *)base)+(next*size)),
               *e2=(void *)(((const char *)base)+((next+1)*size));
    if ( (next+1<nmemb)&&
         ((*compa)(e1,e2)<0) ) {
      next++;
      e1=e2;
    }
    if ((*compa)(e1,e0)<0) {
      break;
    }
    swap((char *)e1,(char *)e0,size);
    pos=next;
    next=2*pos+1;
  }
}
// }}}

void make_heap(void *base,size_t nmemb,size_t size,int (*compa)(const void *,const void *)) // {{{
{
  int iA;
  for (iA=nmemb/2-1;iA>=0;iA--) {
    sink(base,nmemb,size,compa,iA);
  }
}
// }}}

// bubble up nmemb-1
void push_heap(void *base,size_t nmemb,size_t size,int (*compa)(const void *,const void *)) // {{{
{
  size_t pos=nmemb-1;
  while (pos>0) {
    int parent=(pos-1)/2;
    const void *e0=(void *)(((const char *)base)+(parent*size)),
               *e1=(void *)(((const char *)base)+(pos*size));
    if ((*compa)(e1,e0)<0) {
      break;
    }
    swap((char *)e1,(char *)e0,size);
    pos=parent;
  }
}
// }}}

void pop_heap(void *base,size_t nmemb,size_t size,int (*compa)(const void *,const void *)) // {{{
{
  // swap first with last element
  const void *e=(void *)(((const char *)base)+((nmemb-1)*size));
  swap((char *)base,(char *)e,size);
  // fix heap
  sink(base,nmemb-1,size,compa,0);
}
// }}}

void sort_heap(void *base,size_t nmemb,size_t size,int (*compa)(const void *,const void *)) // {{{
{
  while (nmemb>0) {
    pop_heap(base,nmemb,size,compa);
    nmemb--;
  }
}
// }}}

