#include "tjheap.h"
#include "tjstring.h" // for ftos


#ifdef CUSTOM_HEAP


#define MYHEAP_SIZE CUSTOM_HEAP_SIZE*0x100000


void custom_heap_error(const char* txt) {
  if(Heap::tracefunc) Heap::tracefunc(txt);
  else fprintf(stderr,"%s\n",txt);
  abort();
}


//#define USE_TRIVIAL_ALGORITM


#ifdef USE_TRIVIAL_ALGORITM
//////////////////////////////////// START of Trivial Algoritm /////////////////////////////

#define TRIVIAL_HEAP_SIZE 100*MYHEAP_SIZE // Use large heap since it is never freed

static char myheap[TRIVIAL_HEAP_SIZE];
static unsigned int heapcount=0;


void* trivial_alloc(size_t size) {
  if((heapcount+size)>=TRIVIAL_HEAP_SIZE) {
    custom_heap_error("Out of memory");
  }
  void* result=myheap+heapcount;
  heapcount+=size;
  return result;
}


//#ifndef STL_REPLACEMENT
//void* operator new(size_t size) throw (std::bad_alloc) {return trivial_alloc(size);}
//#else
void* operator new(size_t size) {return trivial_alloc(size);}
//#endif

//void* operator new(size_t size) {return qf_malloc(size);}
void* operator new[](size_t size) {return trivial_alloc(size);}

void operator delete(void* mptr) {}
void operator delete[](void* mptr) {}


//////////////////////////////////// END of Trivial Algoritm /////////////////////////////

#else // USE_TRIVIAL_ALGORITM

/////////////////////////////////// START of List Allocator Algoritm /////////////////////

#define ALIGNMENT 8
static char la_heap[MYHEAP_SIZE];

static bool la_init_done=false;

inline size_t la_downalign(size_t ptr) {
  return (ptr/ALIGNMENT)*ALIGNMENT;
}

inline size_t la_upalign(size_t ptr) {
  if(ptr%ALIGNMENT) return (ptr/ALIGNMENT+1)*ALIGNMENT;
  return (ptr/ALIGNMENT)*ALIGNMENT;
}


struct la_cell {

  inline void set_used()   {next=(la_cell*)((size_t)next | 1);} // Set least significant bit
  inline void set_unused() {next=(la_cell*)((size_t)next & ~1);} // Clear least significant bit

  inline bool is_used() const {
    return (size_t)next & 1; // Read least significant bit
  }

  inline size_t size() const {
    if(next<this) return 0; // last cell
    return (size_t)next-(size_t)this-sizeof(la_cell);
  }

  inline void* memptr() const {
    return (void*)((size_t)this+sizeof(la_cell));
  }

  inline la_cell* aligned_prev() {return prev;} // Already aligned
  inline la_cell* aligned_next() {return (la_cell*)la_downalign((size_t)next);}

  la_cell* prev;
  la_cell* next;
};

la_cell* la_rovptr=0;
la_cell* la_begin=0;
la_cell* la_end=0;


/*
void check_integrity(const char* caller, la_cell* cell) {
  la_cell* next=cell->aligned_next();
  la_cell* prev=cell->aligned_prev();

  // Exclude from check
  if(next==la_begin || next==la_end) return;
  if(prev==la_begin || prev==la_end) return;
  if(cell==la_begin || cell==la_end) return;

  if(prev>=cell) {fprintf(stderr,"%s: prev>=cell\n",caller); custom_heap_error("check_integrity failed");}
  if(cell>=next) {fprintf(stderr,"%s: cell>=next\n",caller); custom_heap_error("check_integrity failed");}
}

void la_dump() {
  la_cell* iter=la_begin;
  while(1) {
    fprintf(stdout,"size(%p)=%i\t\t prev=%p\t\t next=%p\t\t used=%i\n",iter,iter->size(),iter->aligned_prev(),iter->aligned_next(),iter->is_used());
    iter=iter->aligned_next();
    if(iter==la_begin) {
      break;
    }
  }
}

*/


inline void la_init() {

  // some checks for implicit assumptions
  if(la_upalign(sizeof(la_cell))!=sizeof(la_cell)) custom_heap_error("la_cell does not align");
  if(sizeof(la_cell)!=(2*sizeof(la_cell*))) custom_heap_error("sizeof(la_cell) exceeds");

  // Place begin cell
  la_begin=(la_cell*)la_upalign((size_t)la_heap);

  // Place end cell
  char* offset=la_heap+MYHEAP_SIZE-2*(ALIGNMENT+sizeof(la_cell)); // leave enough space for un-aligned memory and both cells
  la_end=(la_cell*)la_downalign((size_t)offset);

  // Connect both cells cyclically
  la_begin->prev=la_begin->next=la_end;
  la_end->prev=la_end->next=la_begin;

  la_begin->set_unused(); // This will hold all of the free memory upon startup
  la_end->set_used(); // So last cell will never be modified

  la_rovptr=la_begin; // set to begin of list

  la_init_done=true;
}


inline void* list_alloc(size_t size) {
  if(!la_init_done) la_init();

  size=la_upalign(size);

  // Iterate to next free cell
  la_cell* iter=la_rovptr;
  while(iter->is_used() || iter->size()<size) {
    iter=iter->aligned_next();
    if(iter==la_rovptr) {
//      la_dump();
      custom_heap_error("Out of memory");
    }
  }

  // To split a cell is only useful if it can accomodate
  // some data (i.e. ALIGNMENT) plus one extra cell header
  bool split_cell = ( iter->size() >= (size+ALIGNMENT+sizeof(la_cell)) );

  if(split_cell) {

    la_cell* next=iter->aligned_next();

    // Create new header to hold remainder of cell
    la_cell* remainder= (la_cell*)( (size_t)iter + sizeof(la_cell) + size );

    // Connect cells
    remainder->next=next;
    remainder->prev=iter;
    next->prev=remainder;
    iter->next=remainder;

    remainder->set_unused();
    iter->set_used();

    la_rovptr=remainder; // Set to the (free) remainder

    return iter->memptr();

  } else { // Use entire cell

    iter->set_used();
    la_rovptr=iter->aligned_next(); // Set to the cell after the exact match
    return iter->memptr();
  }

}



inline void delete_cell(la_cell* cell) {
//  check_integrity("delete_cell:cell",cell);

  la_cell* next=cell->aligned_next();
  la_cell* prev=cell->aligned_prev();

  // Connect ends
  prev->next=next;
  next->prev=prev;

  // Alternative: Just jump to freed space if sufficiently large
  // However, this seems to have a somewhat arbitray maximum in
  // performance depending on the threshold, so we will not use it
//  if(prev->size()>16*ALIGNMENT) la_rovptr=prev;
//  else la_rovptr=next;

  la_rovptr=prev; // Set roving pointer to free space. (Found out empirically that this gives a great performance boost)
}


inline void list_free(void *ptr) {

  // Convert pointer to header
  la_cell* freecell = (la_cell*) ( (size_t)ptr - sizeof(la_cell) );

  // Try to join with next
  la_cell* nextcell=freecell->aligned_next();
  if(!nextcell->is_used()) delete_cell(nextcell);

  // Try to join with previous
  if(!freecell->aligned_prev()->is_used()) delete_cell(freecell);
  else freecell->set_unused(); // Otherwise, just reset used bit
}



//#ifndef STL_REPLACEMENT
//void* operator new(size_t size) throw (std::bad_alloc) {return list_alloc(size);}
//#else
void* operator new(size_t size) {return list_alloc(size);}
//#endif

//void* operator new(size_t size) {return qf_malloc(size);}
void* operator new[](size_t size) {return list_alloc(size);}

void operator delete(void* mptr) {list_free(mptr);}
void operator delete[](void* mptr) {list_free(mptr);}


/////////////////////////////////// END of List Allocator Algoritm /////////////////////
#endif // USE_TRIVIAL_ALGORITM

#endif // CUSTOM_HEAP



heaptracefunction Heap::tracefunc=0;


void Heap::malloc_stats() {
#ifdef CUSTOM_HEAP

#ifndef USE_TRIVIAL_ALGORITM
  if(la_end) {
    float used_mb=float(MYHEAP_SIZE-la_end->aligned_prev()->size())/float(0x100000); // Estimate by size of last cell
    if(Heap::tracefunc) Heap::tracefunc(("Memory used: "+ftos(used_mb)+"/"+ftos(CUSTOM_HEAP_SIZE)+" MB").c_str());
  }
#endif


#endif // CUSTOM_HEAP
}