#include "mem2alloc.hpp"

#include <ogc/system.h>
#include <ogc/cache.h>
#include <algorithm>
#include <string.h>
#include "lockMutex.hpp"

void CMEM2Alloc::init(void *addr, void *end)
{
	m_baseAddress = (SBlock *)(((u32)addr + 31) & ~31);
	m_endAddress = (SBlock *)((u32)end & ~31);
	LWP_MutexInit(&m_mutex, 0);
}

void CMEM2Alloc::cleanup(void)
{
	LWP_MutexDestroy(m_mutex);
	m_mutex = 0;
	m_first = 0;
	// Try to release the range we took through SYS functions
	/*if (SYS_GetArena2Lo() == m_endAddress)
		SYS_SetArena2Lo(m_baseAddress);*/
	m_baseAddress = 0;
	m_endAddress = 0;
}

void CMEM2Alloc::clear(void)
{
	m_first = 0;
	u32 Size = (u32)m_endAddress - (u32)m_baseAddress;
	memset(m_baseAddress, 0, Size);
	DCFlushRange(m_baseAddress, Size);
}

unsigned int CMEM2Alloc::usableSize(void *p)
{
	return p == 0 ? 0 : ((SBlock *)p - 1)->s * sizeof (SBlock);
}

void *CMEM2Alloc::allocate(unsigned int s)
{
	if (s == 0)
		s = 1;
	//
	LockMutex lock(m_mutex);
	//
	s = (s - 1) / sizeof (SBlock) + 1;
	// First block
	if (m_first == 0)
	{
		if (m_baseAddress + s + 1 >= m_endAddress)
			return 0;
		m_first = m_baseAddress;
		m_first->next = 0;
		m_first->prev = 0;
		m_first->s = s;
		m_first->f = false;
		return (void *)(m_first + 1);
	}
	// Search for a free block
	SBlock *i;
	SBlock *j = m_first;
	for (i = m_first; i != 0; i = i->next)
	{
		if (i->f && i->s >= s)
			break;
		j = i;
	}
	// Create a new block
	if (i == 0)
	{
		i = j + j->s + 1;
		if (i + s + 1 >= m_endAddress)
			return 0;
		j->next = i;
		i->prev = j;
		i->next = 0;
		i->s = s;
		i->f = false;
		return (void *)(i + 1);
	}
	// Reuse a free block
	i->f = false;
	// Split it
	if (i->s > s + 1)
	{
		j = i + s + 1;
		j->f = true;
		j->s = i->s - s - 1;
		i->s = s;
		j->next = i->next;
		j->prev = i;
		i->next = j;
		if((((u32)j->next) & 0xf0000000) != 0)
			j->next->prev = j;
	}
	return (void *)(i + 1);
}

void CMEM2Alloc::release(void *p)
{
	if (p == 0)
		return;

	LockMutex lock(m_mutex);
	SBlock *i = (SBlock *)p - 1;
	i->f = true;

	// If there are no other blocks following yet,
	// set the remaining size to free size. - Dimok
	if((((u32)i->next) & 0xf0000000) == 0)
		i->s = m_endAddress - i - 1;

	// Merge with previous block
	if ((((u32)i->prev) & 0xf0000000) != 0 && i->prev->f)
	{
		i = i->prev;
		i->s += i->next->s + 1;
		i->next = i->next->next;
		if((((u32)i->next) & 0xf0000000) != 0)
			i->next->prev = i;
	}
	// Merge with next block
	if ((((u32)i->next) & 0xf0000000) != 0 && i->next->f)
	{
		i->s += i->next->s + 1;
		i->next = i->next->next;
		if((((u32)i->next) & 0xf0000000) != 0)
			i->next->prev = i;
	}
}

void *CMEM2Alloc::reallocate(void *p, unsigned int s)
{
	SBlock *i;
	SBlock *j;
	void *n;

	if (s == 0)
		s = 1;
	if (p == 0)
		return allocate(s);

	i = (SBlock *)p - 1;
	s = (s - 1) / sizeof (SBlock) + 1;

	LockMutex lock(m_mutex);

	//out of memory /* Dimok */
	if (i + s + 1 >= m_endAddress)
	{
		return 0;
	}

	// Last block
	if (((((u32)i->next) & 0xf0000000) == 0) && i + s + 1 < m_endAddress)
	{
		i->s = s;
		return p;
	}
	// Size <= current size + next block
	if ((((u32)i->next) & 0xf0000000) != 0 && i->s < s && i->next->f && i->s + i->next->s + 1 >= s)
	{
		// Merge
		i->s += i->next->s + 1;
		i->next = i->next->next;
		if((((u32)i->next) & 0xf0000000) != 0)
			i->next->prev = i;
	}
	// Size <= current size
	if (i->s >= s)
	{
		// Split
		if (i->s > s + 1)
		{
			j = i + s + 1;
			j->f = true;
			j->s = i->s - s - 1;
			i->s = s;
			j->next = i->next;
			j->prev = i;
			i->next = j;
			if((((u32)j->next) & 0xf0000000) != 0)
				j->next->prev = j;
		}
		return p;
	}

	// Size > current size
	n = allocate(s * sizeof (SBlock));
	if (n == 0)
		return 0;
	memcpy(n, p, i->s * sizeof (SBlock));
	release(p);
	return n;
}

unsigned int CMEM2Alloc::FreeSize()
{
	LockMutex lock(m_mutex);

	if (m_first == 0)
		return (u32)m_endAddress - (u32)m_baseAddress;

	SBlock *i;
	unsigned int size = 0;

	for(i = m_first; i != 0; i = i->next)
	{
		if(i->f && (((u32)i->next) & 0xf0000000) != 0)
			size += i->s;

		else if(i->f && (((u32)i->next) & 0xf0000000) == 0)
			size += m_endAddress - i - 1;

		else if(!i->f && (((u32)i->next) & 0xf0000000) == 0)
			size += m_endAddress - i - i->s - 1;
	}

	return size*sizeof(SBlock);
}