#include "BSprivate.h"

/*
   Space management routines
 */

/* 
    Allocator for large numbers of small blocks of equal size:

    Data is double alligned, as is expected by most routines.

    Data structure: 
        A list of blocks.
*/

typedef struct __BSMBlock {
	int	base_size;
	int	num_units;
	double	*top, *bottom;			/* bounds of memory for this block */
									/* used for free'ing quickly */
	struct	__BSMBlock   *next;     /* Pointer to next BSMBlock */
	double	*free_blocks;
	int	num_allocated;
	double v;                        /* Dummy for top */
} BSMBlock;

/* 
    Create a new block of base_size in size
 */
BSMBlock *BSsbinit(long base_size, int num_units)
{
	BSMBlock *B;
	double	*tmp, *oldtmp;
	double	**ptr;
	int	true_base_size, test_size, true_size;
	int	i;

	true_base_size = (base_size + sizeof(double) - 1) / sizeof(double);
		/* double aligned */
	true_base_size++; /* account for next pointers */

	/* try to find the next highest power of two */
	test_size = (num_units*true_base_size)*sizeof(double) + sizeof(BSMBlock);
	true_size = sizeof(double);
	while (true_size-1 < test_size) {
		true_size *= 2;
	}
	true_size--;
	num_units = (true_size - sizeof(BSMBlock)) / sizeof(double);
	num_units /= true_base_size;

	MY_MALLOCN(B,(BSMBlock *),true_size,1);
	B->base_size = base_size;
	B->num_units = num_units;
	B->top       = &B->v;
	B->bottom       = &(B->top[num_units*true_base_size]);
	B->num_allocated = 0;
	B->next      = NULL;

	/* set up the free list */
	tmp = B->top;
	B->free_blocks = tmp;
	oldtmp = tmp;
	for (i=1;i<num_units;i++) {
		tmp += true_base_size;
		ptr = (double **) oldtmp;
		ptr[0] = tmp; /* set next of previous ptr to current */
		oldtmp = tmp;
	}
	ptr = (double **) tmp;
	ptr[0] = NULL;
	return(B);
}

double *BSsbmalloc(BSMBlock **B )
{
	double *p;
	BSMBlock *BB = *B;
	BSMBlock *tmp_next, *prev;
	double	**ptr;
	int	found;

	/* Find a block with free space, or create one */
	found = 0;
	prev = NULL;
	while ((BB != NULL) && (!found)) {
		if (BB->free_blocks != NULL) {
			found = 1;
		} else {
			prev = BB;
			BB = BB->next;
		}
	}
	if (found == 0) {
		BB = (*B);
     	/* get new block */
     	(*B) = BSsbinit( BB->base_size, BB->num_units*2 ); CHKERRN(0);
		if ((*B) == NULL) return(NULL);
     	(*B)->next = BB;
     	BB = (*B);
    } else if (BB != (*B)) {
		tmp_next = BB->next;
		BB->next = (*B);
		prev->next = tmp_next;
		(*B) = BB;
	}

	BB->num_allocated++;
	ptr = (double **) BB->free_blocks;
	p = &(BB->free_blocks[1]);
	BB->free_blocks = ptr[0];
	return(p);
}

/* 
   Free a block allocated with sbmalloc
 */
void BSsbfree(double *p, BSMBlock **B)
{
	BSMBlock *t, *prev;
	double	**ptr;

	prev= NULL;
	t   = *B;
	while (t != NULL) {
    	/* is p in this block ? */
    	if (p >= t->top && p< t->bottom) {
        	t->num_allocated--;
			ptr = (double **) (p - 1);
			ptr[0] = t->free_blocks;
			t->free_blocks = (double *) ptr;
			/* we don't free the main blocks, that is done when BSmem_clean */
			/* is called to clean all the blocks up */
			/* move this block to the front */
			if (prev != NULL) {
				prev->next = t->next;
				t->next = (*B);
				(*B) = t;
			}
        	return;
        }
    	prev = t;
    	t    = (BSMBlock *)t->next;
   	}
	MY_SETERRC(MEM_ERROR,"Bad free, could not find block\n");
}

void	BSmem_clean(BSMBlock **B)
{
	BSMBlock *cur, *prev;

	cur = *B;
	while (cur != NULL) {
		prev = cur;
		cur = cur->next;
		MY_FREE(prev);
	}
	(*B) = NULL;
}