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

uint16_t max_free[(MAX_SIZE-MIN_SIZE)/STRIDE];

void fixed_alloc_init()
{
	int i;
	for(i = 0; i < (MAX_SIZE-MIN_SIZE)/STRIDE; ++i)
		max_free[i] = (BLOCK_SIZE - sizeof(mem_block)+1)*8/((i*STRIDE+MIN_SIZE)*8+1);
}

mem_manager * new_mem_manager()
{
	int i;
	mem_manager * ret = malloc(sizeof(mem_manager));
	memset(ret, 0, sizeof(mem_manager));
	return ret;
}

void * falloc(size_t size, mem_manager * manager)
{
	uint16_t i,bit;
	uint16_t bucket;
	mem_block * block,*temp;
	if(size > MAX_SIZE)
		return malloc(size);
	//puts("falloc");
	size = ADJUST_SIZE(size);
	bucket = (size-MIN_SIZE)/STRIDE;
	block = manager->inuse[bucket];
	if(!block)
	{
		block = manager->freelist;
		if(block)
		{
			--manager->freecount;
			manager->freelist = block->next;
			//memset(block, 0xCD, BLOCK_SIZE);
		}
		else
		{
			block = block_alloc(BLOCK_SIZE);
			//memset(block, 0xAB, BLOCK_SIZE);
		}
		manager->inuse[bucket] = block;
		block->next = NULL;
		block->last = NULL;
		block->numfree = max_free[bucket];
		block->firstfree = 0;
		memset(block->bitmap, 0xFF, max_free[bucket]/8+1);
	}
	//printf("block: %X,numfree: %d, firstfree: %d, maxfree: %d\n", block, block->numfree, block->firstfree, max_free[bucket]);
	/*if(block->numfree > max_free[bucket])
	{
		puts("uh oh!");
		exit(1);
	}*/
	//find first free 
	i = block->firstfree;
	while(!block->bitmap[i])
		++i;
	//printf("i:%d,bitmap:%X\n", i, block->bitmap[i]);
	bit = 0;
	while(!((1 << bit) & block->bitmap[i]))
		++bit;
	//update free bitmask
	block->bitmap[i] ^= 1 << bit;
	//If current bitmap has no more free elements, set firstfree to the next bitmap
	if(!block->bitmap[i])
		block->firstfree = i+1;
	else
		block->firstfree = i;
	--block->numfree;
	if(!block->numfree)
	{
		//Remove from linked list if there are no more free elements
		manager->inuse[bucket] = block->next;
		if(block->next)
			block->next->last = block->last;
	}
	i = i*8+bit;
	return (void *)(((char *)block)+BLOCK_SIZE-((i+1)*size));
}

void ffree(void * ptr, size_t size, mem_manager * manager)
{
	mem_block * block,*temp;
	uint16_t i,bit,bucket;
	if(size > MAX_SIZE)
	{
		free(ptr);
		return;
	}
	//puts("ffree");
	size = ADJUST_SIZE(size);
	//memset(ptr, 0xEF, size);
	block = GET_BLOCK(ptr);
	i = (((((char *)block) + BLOCK_SIZE) - ((char *)ptr))/size)-1;
	bit = i & 0x7;
	i = (i&0xFFFFFFF8) >> 3;
	//printf("ptr:%X,block:%X,i:%d,bit:%d\n", ptr,block,bit,i);
	block->bitmap[i] |= 1 << bit;
	if(i < block->firstfree)
		block->firstfree = i;
	++block->numfree;
	bucket = (size-MIN_SIZE)/STRIDE;
	//printf("numfree:%d,max_free:%d,last:%X,next:%X\n", block->numfree, max_free[bucket], block->last, block->next);
	/*if(block->numfree > max_free[bucket])
	{
		puts("uh oh!");
		exit(1);
	}*/
	if(block->numfree == max_free[bucket])
	{
		//Block is now unused, remove it from the inuse list
		if(block->last)
			block->last->next = block->next;
		else
			manager->inuse[bucket] = block->next;
		if(block->next)
			block->next->last = block->last;
		if(manager->freecount == MAX_FREE)
		{
			block_free(block, BLOCK_SIZE);
		}
		else
		{
			block->next = manager->freelist;
			manager->freelist = block;
			++manager->freecount;
		}
	} else if(block->numfree == 1) {
		//Block was previously full, add it to the inuse list
		block->next = manager->inuse[bucket];
		block->last = NULL;
		manager->inuse[bucket] = block;
		if(block->next)
			block->next->last = block;
	} else if(block->next && block->next->numfree < block->numfree) {
		//We want to use more filled blockes before less filled ones
		//so we can return empty ones to the OS more often
		//so if we now have more free nodes in this block than the 
		//next one swap them
		temp = block->next;
		block->next = temp->next;
		temp->last = block->last;
		block->last = temp;
		temp->next = block;
		if(block->next)
			block->next->last = block;
		if(temp->last)
			temp->last->next = temp;
		else
			manager->inuse[bucket] = temp;
	}
}