#include "mem.h"

//#include <stdlib.h>
//#include <stdio.h>
#include <string.h>

#include "kernel.h"
#include "swis.h"

#include "log.h"

typedef struct
{
	int	tag;
	int	free;
	int	base;
	int	end;
} Heap;

static struct
{
	Heap*	m_pHeap;
	int	m_PageSize;
	int	m_HeapSize;
	int	m_StartSize;
	int	m_flags;
	int	m_AreaId;
} TheHeap = {NULL, 0, 0, 0, 0};

_kernel_oserror* mem_init(int flags)
{
	_kernel_oserror* err;

	flags = flags;

	if (TheHeap.m_flags & EMem_Init) return NULL;

	TheHeap.m_flags = EMem_Init;

	// OS_ReadMemMapInfo, read page size
	if (_swix(OS_ReadMemMapInfo, _OUT(0), &TheHeap.m_PageSize) != NULL)
		TheHeap.m_PageSize = 4096;

	// Allocate a dynamic area
	err = _swix( OS_DynamicArea, _INR(0,8)|_OUT(1)|_OUT(3)
		   , 0, -1, TheHeap.m_PageSize, -1, 0x81
		   , 16*1024*1024, 0, 0, "Vorbis"
		   , &TheHeap.m_AreaId, &TheHeap.m_pHeap);
	if (err) return err;

	// Read size to be on the safe side
	err = _swix( OS_DynamicArea, _INR(0,1)|_OUT(2)
	           , 2, TheHeap.m_AreaId, &TheHeap.m_HeapSize);
	if (err) return err;

	// Initialise heap
	err = _swix(OS_Heap, _INR(0,3), 0, TheHeap.m_pHeap, 0, TheHeap.m_HeapSize);
	if (err) return err;

	TheHeap.m_flags |= EMem_Init;

	return NULL;
}

void mem_finalise(void)
{
	if (!(TheHeap.m_flags & EMem_Init)) return;

	// Release dynamic area
	_swix( OS_DynamicArea, _INR(0,1), 1, TheHeap.m_AreaId);

	TheHeap.m_flags &= ~EMem_Init;
}

static void mem_mapheap(void)
{
	int size = TheHeap.m_HeapSize;

	// Read new size of dynamic area (cf rounding to memory pages)
	_swix( OS_DynamicArea, _INR(0,1)|_OUT(2)
	     , 2, TheHeap.m_AreaId, &TheHeap.m_HeapSize);

	// extend heap to fit exactly size of dynamic area
	_swix( OS_Heap, _INR(0,3), 5, TheHeap.m_pHeap, 0, TheHeap.m_HeapSize - size);
}

void mem_pack(void)
{
	_kernel_swi_regs regs;
	int delta;

	if (!(TheHeap.m_flags & EMem_Pack)) return;

	TheHeap.m_flags &= ~EMem_Pack;

	// shrink heap as far as possible
	regs.r[0] = 5;
	regs.r[1] = (int) TheHeap.m_pHeap;
	regs.r[2] = 0;
	regs.r[3] = -TheHeap.m_HeapSize;
	_kernel_swi(XOS_Bit + OS_Heap, &regs, &regs);
	delta = regs.r[3];
	TheHeap.m_HeapSize -= delta;

	// attempt to shrink dynamic area by a similar value
	_swix( OS_ChangeDynamicArea, _INR(0,1), TheHeap.m_AreaId, -delta);

	// fit heap and dynamic area
	mem_mapheap();
}

void* __mem_realloc(const char* filename, int atline, void* pdata, int newsize)
{
	void*			p;
	int			size;

	if (!pdata) return __mem_alloc(filename, atline, newsize);

	if (newsize == 0)
		newsize = 1;

	// OS_Heap, read block size
	_swi(OS_Heap, _INR(0,2)|_OUT(3), 6, TheHeap.m_pHeap, pdata, &size);
	size -= 4;

	p = __mem_alloc(filename, atline, newsize);
	if (p)
	{
		if (size > newsize) size = newsize;
		memcpy(p, pdata, size);
	}
	__mem_free(filename, atline, pdata);

	return p;
}

void* __mem_calloc(const char* filename, int atline, int amount, int size)
{
	char* p = __mem_alloc(filename, atline, amount * size);

	if (p) memset(p, 0, amount * size);

	return p;
}

void* __mem_alloc(const char* filename, int atline, int size)
{
	_kernel_oserror* err;
	void*	ptr;
	int	delta;

	if (size == 0) size = 1;

	// Request OS_Heap block
	err = _swix(OS_Heap, _INR(0,3) | _OUT(2), 2, TheHeap.m_pHeap, 0, size, &ptr);

	if (err)
	{
		// Not enough memory, extend dynamic area
		delta = 1 + size / TheHeap.m_PageSize;
		if (size % TheHeap.m_PageSize) delta++;
		delta *= TheHeap.m_PageSize;
		err = _swix(OS_ChangeDynamicArea, _INR(0,1), TheHeap.m_AreaId, delta);
		if (err) Log("%d %s", delta, err->errmess);

		// fit heap and dynamic area
		mem_mapheap();
		// Request block again
		err= _swix(OS_Heap, _INR(0,3) | _OUT(2)
		          , 2, TheHeap.m_pHeap, 0, size, &ptr);
		if (err)
		{
			Log("%s at line %d: %s", filename, atline, err->errmess);
			return NULL;
		}
	}

	return ptr;
}

void __mem_free(const char* filename, int atline, const void* pdata)
{
	_kernel_oserror* err;
	if (!pdata) return;

	// OS_Heap, free block
	err = _swix(OS_Heap, _INR(0,2), 3, TheHeap.m_pHeap, pdata);
	if (err) Log("%s", err->errmess);

	TheHeap.m_flags |= EMem_Pack;
}
