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

#undef MATCHING
#define MATCHING

static const int heap_offset = 8;

THeap *obj_heap;
THeap *alt_heap;

void heap_xfree(void *ptr) {

}

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

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 ((heap_node *)heap_ptr)
	const u8 *ptr_u8 = (u8 *)ptr;
	if (ptr == NULL) {
		return;
	} else {
		heap_node *prev_node = heap_nodes;
		heap_node *node;

		for (; node = prev_node->next, node < ptr_heap; prev_node = node);

		if (((u8 *)&ptr_heap->next + ptr_heap->remaining_size) == (u8 *)&node->next) {
			ptr_heap->next = node->next;
			ptr_heap->remaining_size += node->remaining_size;
		} else {
			ptr_heap->next = node;
		}

		if (((u8 *)&prev_node->next + prev_node->remaining_size) == heap_ptr) {
			prev_node->next = ptr_heap->next;
			prev_node->remaining_size += ptr_heap->remaining_size;
		} else {
			prev_node->next = ptr_heap;
		}
	}
	#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) {
	heap_node *prev_node = heap_nodes;
	heap_node *node;
	u32 size_align = (heap_offset - 1) + align + size;
	u32 aligned_size = size_align & -align;
	goto start;

	cond:
	if (node->remaining_size < aligned_size) {
		goto loop_body;
	}

	top:
	if (aligned_size == node->remaining_size) {
		prev_node->heap = node->heap;
	} else {
		u8 *next_u8 = (u8 *)&node->next;
		heap_node *tmp = &next_u8[aligned_size];
		tmp->next = node->next;
		tmp->remaining_size = node->remaining_size - aligned_size;
		node->remaining_size = aligned_size;
		prev_node->next = tmp;
	}
	return node + 1;

	loop_body:
	prev_node = node;
	start:
	if (node = prev_node->next, node == 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_nodes);
}

THeap::THeap(size_t size, int alignment) {
	mbr_0x0C = 0;
	mbr_0x10 = 0;
	align = alignment;
	heap_nodes = (heap_node *)xmalloc(size);
	if (heap_nodes != NULL) {
		heap_node *tmp_node;
		heap_node *next_tmp_node;

		memset(heap_nodes, 0, size);

		tmp_node = heap_nodes;
		next_tmp_node = &tmp_node[1];
		tmp_node->next = next_tmp_node;

		heap_nodes->remaining_size = 0;

		next_tmp_node->next = NULL;
		next_tmp_node->remaining_size = size - heap_offset;
	}
}