#include <sys/malloc.h>
#include <stdio.h>

pmmalloc_hunk malloc_first;

void debug( char *fun, char *msg )
{
/*	return; */
/*	printf("%s: %s\n", fun, msg );*/
}

BYTE malloc_init(void)
{
	if (malloc_first->magic!=MALLOC_MAGIC) {
		/* Init by setting up the first hunk */

		debug("malloc_init", "Setting up");
		malloc_first = (pmmalloc_hunk)&malloc_heap_start;

		malloc_first->next = NULL;
		malloc_first->size = 0xDFFFU - 0x200 - sizeof(mmalloc_hunk) - (UWORD)&malloc_heap_start;
		malloc_first->status = MALLOC_FREE;

		malloc_first->magic = MALLOC_MAGIC;
		return 0;
	}
	return -1;
}

void malloc_gc(void)
{
	/* Do a garbage collect on the hunks to create bigger hunks */
	/* Note: assumes that hunks are consecutive */

	pmmalloc_hunk thisHunk, nextHunk;
	UBYTE changed;

	changed = 1;

	debug("malloc_gc","Running");
	while (changed) {
		thisHunk = malloc_first;
		changed = 0;
		while (thisHunk && (thisHunk->magic==MALLOC_MAGIC)) {
			if (thisHunk->status == MALLOC_FREE) {
				nextHunk = thisHunk->next;
				if (nextHunk->status == MALLOC_FREE) {
					/* Must be consecutive */
					changed = 1;
					thisHunk->size+=nextHunk->size+sizeof(mmalloc_hunk);
					thisHunk->next = nextHunk->next;
				}
			}
			thisHunk=thisHunk->next;
		}
		if (thisHunk!=NULL)
			debug("malloc_gc", "Corrupted malloc list found.");
	}
}
		
void *malloc( UWORD size )
{
	pmmalloc_hunk thisHunk, insertBefore;
	pmmalloc_hunk newHunk;
	UBYTE firstTry;

	if (malloc_first->magic != MALLOC_MAGIC)
		malloc_init();

	firstTry = 1;	/* Allows gc if no big enough hunk is found */
	
	while (firstTry) {
		thisHunk = malloc_first;
		if (firstTry == 2)
			firstTry = 0;

		while (thisHunk&&(thisHunk->magic == MALLOC_MAGIC)) {
			debug("malloc", "Entering hunk" );
			if (thisHunk->status == MALLOC_FREE) {
				debug("malloc", "Found free hunk" );

				/* Free, is it big enough? */
				if (thisHunk->size >= size+sizeof(mmalloc_hunk)) {
					
					debug("malloc","Found a big enough hunk.");
					
					/* Yes, big enough */
					insertBefore = thisHunk->next;
					newHunk = (pmmalloc_hunk)((UWORD)thisHunk + size + sizeof(mmalloc_hunk));
					newHunk->size = thisHunk->size - sizeof(mmalloc_hunk) - size;
					newHunk->status = MALLOC_FREE;
					newHunk->next = insertBefore;
					newHunk->magic = MALLOC_MAGIC;
					
					thisHunk->size = size;
					thisHunk->status = MALLOC_USED;
					thisHunk->next = newHunk;
					
					return (void *)((UWORD)thisHunk + sizeof(mmalloc_hunk));
				}
			}
			thisHunk = thisHunk->next;
		}
		if (firstTry) {
			/* Try again after a garbage collect */
			malloc_gc();
			firstTry = 2;
		}
	}
	return NULL;
}