#include "pso/THeap.h"
#include <stdlib.h>
#include <string.h>
#include <global_types.h>

#undef MATCHING
#define MATCHING

static const int heap_offset = 8;

void xfree(void *ptr) {

}

void *xmalloc(size_t size) {
	return NULL;
}

void THeap::heap_free(void *ptr) {
	#define heap_ptr &ptr_u8[-heap_offset]
	#define ptr_heap ((THeap *)heap_ptr)
	const u8 *ptr_u8 = (u8 *)ptr;
	if (ptr == NULL) {
		return;
	} else {
		THeap *parent = (THeap *)heap;
		THeap *child;

		for (; child = (THeap *)parent->heap, child < ptr_heap; parent = child);

		if (((u8 *)&ptr_heap->heap + ptr_heap->alloc_size) == (u8 *)&child->heap) {
			ptr_heap->heap = child->heap;
			ptr_heap->alloc_size += child->alloc_size;
		} else {
			ptr_heap->heap = (u8 *)child;
		}

		if (((u8 *)&parent->heap + parent->alloc_size) == heap_ptr) {
			parent->heap = ptr_heap->heap;
			parent->alloc_size += ptr_heap->alloc_size;
		} else {
			parent->heap = (u8 *)heap_ptr;
		}
	}
	#undef ptr_heap
	#undef heap_ptr
}

void *THeap::heap_zalloc(size_t size) {
	void *ptr = heap_alloc(size);
	if (ptr != NULL) {
		memset(ptr, 0, size);
	}
	return ptr;
}

// Getting closer, but register allocation issues.
#ifndef MATCHING
void *THeap::heap_alloc(size_t size) {
	THeap *parent = (THeap *)heap;
	THeap *child;
	u32 size_align = (heap_offset - 1) + align + size;
	u32 aligned_size = size_align & -align;
	goto start;

	cond:
	if (child->alloc_size < aligned_size) {
		goto loop_body;
	}

	top:
	if (aligned_size == child->alloc_size) {
		parent->heap = child->heap;
	} else {
		THeap *tmp = (THeap *)&child->heap[aligned_size];
		tmp->heap = child->heap;
		tmp->alloc_size = child->alloc_size - aligned_size;
		child->alloc_size = aligned_size;
		parent->heap = (u8 *)&tmp->heap;
	}
	return (void *)&child->align;

	loop_body:
	parent = child;
	start:
	if (child = (THeap *)parent->heap, child == NULL) {
		return NULL;
	}
	goto cond;
}
#else
asm void *THeap::heap_alloc(size_t size) {
nofralloc
/* 80141028 80030008 */ lwz         r0, 8(r3)
/* 8014102C 80630000 */ lwz         r3, 0(r3)
/* 80141030 7CC02214 */ add         r6, r0, r4
/* 80141034 7C0000D0 */ neg         r0, r0
/* 80141038 38C60007 */ addi        r6, r6, 0x7
/* 8014103C 7CC60038 */ and         r6, r6, r0
/* 80141040 48000050 */ b           lbl_80141090
lbl_80141044:
/* 80141044 80040004 */ lwz         r0, 4(r4)
/* 80141048 7C003040 */ cmplw       r0, r6
/* 8014104C 41800040 */ blt-        lbl_8014108c
/* 80141050 7C060040 */ cmplw       r6, r0
/* 80141054 40820010 */ bne-        lbl_80141064
/* 80141058 80040000 */ lwz         r0, 0(r4)
/* 8014105C 90030000 */ stw         r0, 0(r3)
/* 80141060 48000024 */ b           lbl_80141084
lbl_80141064:
/* 80141064 80040000 */ lwz         r0, 0(r4)
/* 80141068 7CA43214 */ add         r5, r4, r6
/* 8014106C 90050000 */ stw         r0, 0(r5)
/* 80141070 80040004 */ lwz         r0, 4(r4)
/* 80141074 7C060050 */ subf        r0, r6, r0
/* 80141078 90050004 */ stw         r0, 4(r5)
/* 8014107C 90C40004 */ stw         r6, 4(r4)
/* 80141080 90A30000 */ stw         r5, 0(r3)
lbl_80141084:
/* 80141084 38640008 */ addi        r3, r4, 0x8
/* 80141088 4E800020 */ blr
lbl_8014108c:
/* 8014108C 7C832378 */ mr          r3, r4
lbl_80141090:
/* 80141090 80830000 */ lwz         r4, 0(r3)
/* 80141094 28040000 */ cmplwi      r4, 0
/* 80141098 4082FFAC */ bne+        lbl_80141044
/* 8014109C 38600000 */ li          r3, 0x0
/* 801410A0 4E800020 */ blr
}
#endif

THeap::~THeap() {
	xfree(heap);
}

// Getting closer, still not sure how to match though.
#ifndef MATCHING
THeap::THeap(size_t size, int alignment) {
	heap_size = 0;
	mbr_0x10 = 0;
	align = alignment;
	heap = (u8 *)xmalloc(heap_size);
	if (heap != NULL) {
		const u32 new_size = size - heap_offset;
		THeap *tmp_heap;
		memset(heap, 0, size);

		tmp_heap = (THeap *)heap;
		tmp_heap->heap = &heap[heap_offset];
		((THeap *)heap)->alloc_size = 0;
		tmp_heap->align = 0;
		tmp_heap->heap_size = new_size;
	}
}
#else
asm THeap::THeap(size_t size, int alignment) {
nofralloc
/* 801410F0 9421FFF0 */ stwu        r1, -0x10(r1)
/* 801410F4 7C0802A6 */ mflr        r0
/* 801410F8 90010014 */ stw         r0, 0x14(r1)
/* 801410FC 38000000 */ li          r0, 0x0
/* 80141100 BFC10008 */ stmw        r30, 8(r1)
/* 80141104 7C7E1B78 */ mr          r30, r3
/* 80141108 7C9F2378 */ mr          r31, r4
/* 8014110C 9003000C */ stw         r0, 0xc(r3)
/* 80141110 7FE3FB78 */ mr          r3, r31
/* 80141114 901E0010 */ stw         r0, 0x10(r30)
/* 80141118 90BE0008 */ stw         r5, 8(r30)
/* 8014111C 48299FC5 */ bl          xmalloc
/* 80141120 907E0000 */ stw         r3, 0(r30)
/* 80141124 807E0000 */ lwz         r3, 0(r30)
/* 80141128 28030000 */ cmplwi      r3, 0
/* 8014112C 41820034 */ beq-        lbl_80141160
/* 80141130 7FE5FB78 */ mr          r5, r31
/* 80141134 38800000 */ li          r4, 0x0
/* 80141138 4BECD1FD */ bl          memset
/* 8014113C 807E0000 */ lwz         r3, 0(r30)
/* 80141140 38800000 */ li          r4, 0x0
/* 80141144 381FFFF8 */ addi        r0, r31, -0x8
/* 80141148 38A30008 */ addi        r5, r3, 0x8
/* 8014114C 90A30000 */ stw         r5, 0(r3)
/* 80141150 807E0000 */ lwz         r3, 0(r30)
/* 80141154 90830004 */ stw         r4, 4(r3)
/* 80141158 90850000 */ stw         r4, 0(r5)
/* 8014115C 90050004 */ stw         r0, 4(r5)
lbl_80141160:
/* 80141160 7FC3F378 */ mr          r3, r30
/* 80141164 BBC10008 */ lmw         r30, 8(r1)
/* 80141168 80010014 */ lwz         r0, 0x14(r1)
/* 8014116C 7C0803A6 */ mtlr        r0
/* 80141170 38210010 */ addi        r1, r1, 0x10
/* 80141174 4E800020 */ blr
}
#endif