#include <malloc.h>


static void rtmalloc_insert_in_free_list(rtmalloc_context_t *context, rtmalloc_block_link_t *block)
{
	rtmalloc_block_link_t *i;

	for(i = &context->free_list_head; i->next < block; i = i->next);

	//check if the block and the previous one are contiguous
	if (((uint8_t*)i + i->size) == (uint8_t*)block)
	{
		i->size += block->size;
		block = i;
	}

	//check if is contigous with the next block
	if (((uint8_t*)block + block->size) == (uint8_t*)i->next)
	{
		if (i->next != context->free_list_tail)
		{
			block->size += i->next->size;
			block->next = i->next->next;
		}
		else
		{
			block->next = context->free_list_tail;
		}
	}
	else
	{
		block->next = i->next;
	}

	if (i != block)
		i->next = block;
}



void rtmalloc_init(rtmalloc_context_t *context, void *heap, size_t size, size_t alignment)
{
	context->alignment = alignment;
	context->alignment_mask = context->alignment - 1;
	context->allocated_mask = (size_t)1 << ((sizeof(size_t) << 3) - 1);
	context->min_block_size = (sizeof(rtmalloc_block_link_t) + context->alignment) & ~context->alignment_mask;

	//align properly
	void *aligned_heap = (void*)(((size_t)heap + context->alignment_mask) & ~context->alignment_mask);
	size -= (aligned_heap - heap);

	context->free_list_head.size = 0;
	context->free_list_head.next = aligned_heap;

	context->free_list_tail = (rtmalloc_block_link_t*)(((size_t)aligned_heap + size - sizeof(rtmalloc_block_link_t)) & ~context->alignment_mask);
	context->free_list_tail->size = 0;
	context->free_list_tail->next = NULL;

	context->size_free = (uint8_t*)context->free_list_tail - (uint8_t*)context->free_list_head.next;
	context->size_free_min = context->size_free;

	context->free_list_head.next->size = context->size_free;
	context->free_list_head.next->next = context->free_list_tail;
}




void* rtmalloc(rtmalloc_context_t *context, size_t size)
{
	void *p = NULL;

	if ((size & context->allocated_mask) == 0)
	{
		if (size > 0)
		{
			size += sizeof(rtmalloc_block_link_t);
			size = (size + context->alignment_mask) & ~context->alignment_mask;
		}

		if ((size > 0) && (size <= context->size_free))
		{
			rtmalloc_block_link_t *prev_block = &context->free_list_head;
			rtmalloc_block_link_t *block = context->free_list_head.next;

			while((block->size < size) && block->next)
			{
				prev_block = block;
				block = block->next;
			}

			if (block != context->free_list_tail)
			{
				//get the memory pointer to return
				p = (uint8_t*)block + sizeof(rtmalloc_block_link_t);

				//remove from list
				prev_block->next = block->next;

				if ((block->size - size) > context->min_block_size)
				{
					//split a larger block
					rtmalloc_block_link_t *new_block_link = (rtmalloc_block_link_t*)((uint8_t*)block + size);
					new_block_link->size = block->size - size;
					block->size = size;

					rtmalloc_insert_in_free_list(context, new_block_link);
				}

				context->size_free -= block->size;
				if (context->size_free < context->size_free_min)
					context->size_free_min = context->size_free;

				block->size |= context->allocated_mask;
				block->next = NULL;
			}
		}
	}

	return p;
}


void rtfree(rtmalloc_context_t *context, void *p)
{
	if (p)
	{
		rtmalloc_block_link_t *block = (rtmalloc_block_link_t*)((uint8_t*)p - sizeof(rtmalloc_block_link_t));

		if ((block->size & context->allocated_mask) && (block->next == NULL))
		{
			block->size &= ~context->allocated_mask;

			context->size_free += block->size;
			rtmalloc_insert_in_free_list(context, block);
		}
	}
}


size_t rtmsize(rtmalloc_context_t *context, void *p)
{
	size_t size = 0;

	if (p)
	{
		rtmalloc_block_link_t *block = (rtmalloc_block_link_t*)((uint8_t*)p - sizeof(rtmalloc_block_link_t));

		if ((block->size & context->allocated_mask) && (block->next == NULL))
			size = block->size & ~context->allocated_mask;
	}

	return size;
}


/*
void rtmalloc_dump(malloc_context_t *context)
{
	printf("\nFree block list (%lu)", context->size_free);

	malloc_block_link_t *block = context->free_list_head.next;

	while(block != context->free_list_tail)
	{
		printf("\n\t%p (%lu)", block, block->size);
		block = block->next;
	}

	printf("\n\n");
}
*/