#include <stdlib.h>
#include <stdint.h>

#include "interface.h"
#include "task.h"
#include "symbols.h"
#include "mem.h"

extern struct task *current_task;

#ifdef USE_MALLOC
#else
static uint16_t memsize
	= MEMSIZE - 1 - (MEMSIZE - 1) % sizeof (struct tasktree);
static uint8_t mtmem[MEMSIZE] = {0};
#endif

//Variable pointers
uint16_t mem_task;
uint16_t mem_heap;

#define TASK_SIZE1(rw, bc,sds, peripherals) \
	(sizeof(struct task) + (rw)*2 + (bc) + (sds) + (peripherals)*(PTRSIZE))
#define TASK_SIZE(t) \
	TASK_SIZE1((t)->returnwidth,\
		(t)->bclen, (t)->sdslen, (t)->peripherals)

size_t mem_freeb(void)
{
	return (size_t)mem_heap - (size_t)mem_task;
}

uint16_t *mem_stack(void)
{
	return nextaligned(uint16_t, mtmem+mem_task);
}

uint16_t mem_max_sp(void)
{
	return mem_heap;
}

void mem_reset(void)
{
	mem_task = 0;
	mem_heap = memsize;
}

void mem_report(void)
{
	msg_log(SC("sizeof(struct task)    : %02x\n"), sizeof(struct task));
	msg_log(SC("sizeof(struct tasktree): %02x\n"),
		sizeof(struct tasktree));
	msg_log(SC("mem_top: %p\n"), mem_ptr(memsize));
	msg_log(SC("mem_bottom: %p\n"), mem_ptr(0));
	msg_log(SC("free bytes: %p\n"), mem_freeb());
}

struct task *mem_alloc_task(
	uint8_t taskid,
	uint8_t returnwidth, uint16_t sdslen, uint16_t bc, uint8_t peripherals)
{
	size_t size = TASK_SIZE1(returnwidth, sdslen, bc, peripherals);
	msg_debug(SC("Alloc task of size %lu\n"), size);

	if (mem_freeb() < size) {
		msg_log(SC("Not enough memory (%lu) need: %lu\n"),
			mem_freeb(), size);
		write_byte(MTFEXCEPTION);
		write_byte(MTEOUTOFMEMORY);
		write_byte(taskid);
		write_byte('\n');
		return NULL;
	}

	//Allocate and ininialize
	struct task *t = mem_cast_task(mem_task);
	t->returnwidth = returnwidth;
	t->sdslen = sdslen;
	t->bclen = bc;
	t->peripherals = peripherals;

	//Increase stack pointer
	mem_task += size;

#ifdef REQUIRE_ALIGNED_MEMORY_ACCESS
	while (mem_task % (sizeof (struct task)) != 0) {
		mem_task++;
		t->bclen++;
		msg_debug(SC("increase bclen to: %lu\n"), t->bclen);
	}
#endif

	return t;
}

struct tasktree *mem_alloc_tree()
{
	mem_heap -= sizeof(struct tasktree);
	msg_debug(SC("tree alloced at: %p\n"), mem_cast_tree(mem_heap));

	if (mem_heap < mem_task) {
		msg_log(SC("Not enough heap\n"));
		write_byte(MTFEXCEPTION);
		write_byte(MTEHEAPUNDERFLOW);
		write_byte(current_task->taskid);
		write_byte('\n');
		return NULL;
	}

	return mem_cast_tree(mem_heap);
}

struct task *mem_task_head()
{
	return mem_task == 0 ? NULL : mem_cast_task(0);
}

struct task *mem_task_next(struct task *t)
{
	struct task *next = (struct task *)(((uint8_t *)t) + TASK_SIZE(t));
	return next >= mem_cast_task(mem_task) ? NULL : next;
}

void *mem_ptr(uint16_t i)
{
	return i == MT_NULL ? NULL : (void *)(mtmem + i);
}

uint16_t mem_rptr(void *p)
{
	return p == NULL ? MT_NULL : ((uint8_t *)p)-mtmem;
}

#if LOGLEVEL == 2
void mem_print_heap()
{
	uint16_t walker;
	walker = memsize - sizeof(struct tasktree);
	while (walker >= mem_heap) {
		struct tasktree *t = mem_cast_tree(walker);
		msg_debug(SC("%d (rptr: %d, trash: %d): "),
			walker, t->ptr, t->trash);
		tasktree_print_node(t);
		walker -= sizeof(struct tasktree);
		msg_debug(SC("\n"));
	}
}

void mem_print_task_heap()
{
	struct task *t = mem_task_head();
	while (t != NULL) {
		msg_debug(SC("task %p %u mem_task: %u size: %u\n"),
			t, mem_rptr(t), mem_task, TASK_SIZE(t));
		msg_debug(SC("id: %u\n"), t->taskid);
		msg_debug(SC("stability: %u\n"), t->stability);
		msg_debug(SC("returnwidth: %u\n"), t->returnwidth);
		msg_debug(SC("bclen: %u\n"), t->bclen);
		msg_debug(SC("sdslen: %u\n"), t->sdslen);
		msg_debug(SC("peripherals: %u\n"), t->peripherals);
		msg_debug(SC("tree: %u\n"), t->tree);

		t = mem_task_next(t);
	}
}
#endif

void mem_node_move(struct tasktree *t, struct tasktree *f)
{
	msg_debug(SC("move %lu - %lu\n"), mem_rptr(t), mem_rptr(f));
	mem_print_heap();
	//Copy the data
	t->task_type = f->task_type;
	t->trash = 0;
	t->data = f->data;
	//We leave the ptr in tact
	//t->ptr = f->ptr;

	//Set the rptrs
	switch (t->task_type) {
	case BCSTABLENODE:
		mem_cast_tree(t->data.stablenode.next)->ptr = mem_rptr(t);
		break;
	case BCUNSTABLENODE:
		mem_cast_tree(t->data.unstablenode.next)->ptr = mem_rptr(t);
		break;
	case BCSTEP:
		mem_cast_tree(t->data.step.lhs)->ptr = mem_rptr(t);
		break;
	case BCSTEPSTABLE:
		mem_cast_tree(t->data.steps.lhs)->ptr = mem_rptr(t);
		break;
	case BCSTEPUNSTABLE:
		mem_cast_tree(t->data.stepu.lhs)->ptr = mem_rptr(t);
		break;
	case BCSEQSTABLE:
		mem_cast_tree(t->data.seqs.lhs)->ptr = mem_rptr(t);
		mem_cast_tree(t->data.seqs.rhs)->ptr = mem_rptr(t);
		break;
	case BCSEQUNSTABLE:
		mem_cast_tree(t->data.sequ.lhs)->ptr = mem_rptr(t);
		mem_cast_tree(t->data.sequ.rhs)->ptr = mem_rptr(t);
		break;
	case BCREPEAT:
		if (t->data.repeat.tree != MT_NULL)
			mem_cast_tree(t->data.repeat.tree)->ptr = mem_rptr(t);
		mem_cast_tree(t->data.repeat.oldtree)->ptr = mem_rptr(t);
		break;
	case BCTOR:
		mem_cast_tree(t->data.tor.lhs)->ptr = mem_rptr(t);
		mem_cast_tree(t->data.tor.rhs)->ptr = mem_rptr(t);
		break;
	case BCTAND:
		mem_cast_tree(t->data.tand.lhs)->ptr = mem_rptr(t);
		mem_cast_tree(t->data.tand.rhs)->ptr = mem_rptr(t);
		break;
	case BCSDSSET:
		mem_cast_tree(t->data.sdsset.data)->ptr = mem_rptr(t);
		break;
	default:
		break;
	}

	//Mark trash (non recursively)
	if (mem_rptr(f) >= mem_heap)
		mem_mark_trash(mem_rptr(f));
	msg_debug(SC("done moving\n"));
	mem_print_heap();
}

void mem_update_ptr(struct tasktree *t, uint16_t old, uint16_t new)
{
	switch (t->task_type) {
	case BCSTABLENODE:
		t->data.stablenode.next = new;
		break;
	case BCUNSTABLENODE:
		t->data.unstablenode.next = new;
		break;
	case BCSTEP:
		t->data.step.lhs = new;
		break;
	case BCSTEPSTABLE:
		t->data.steps.lhs = new;
		break;
	case BCSTEPUNSTABLE:
		t->data.stepu.lhs = new;
		break;
	case BCSEQSTABLE:
		t->data.seqs.lhs =
			t->data.seqs.lhs == old ? new : t->data.seqs.lhs;
		t->data.seqs.rhs =
			t->data.seqs.rhs == old ? new : t->data.seqs.rhs;
		break;
	case BCSEQUNSTABLE:
		t->data.sequ.lhs =
			t->data.sequ.lhs == old ? new : t->data.sequ.lhs;
		t->data.sequ.rhs =
			t->data.sequ.rhs == old ? new : t->data.sequ.rhs;
		break;
	case BCREPEAT:
		t->data.repeat.tree =
			t->data.repeat.tree == old ? new : t->data.repeat.tree;
		t->data.repeat.oldtree =
			t->data.repeat.oldtree == old
			? new : t->data.repeat.oldtree;
		break;
	case BCTOR:
		t->data.tor.lhs =
			t->data.tor.lhs == old ? new : t->data.tor.lhs;
		t->data.tor.rhs =
			t->data.tor.rhs == old ? new : t->data.tor.rhs;
		break;
	case BCTAND:
		t->data.tand.lhs =
			t->data.tand.lhs == old ? new : t->data.tand.lhs;
		t->data.tand.rhs =
			t->data.tand.rhs == old ? new : t->data.tand.rhs;
		break;
	case BCSDSSET:
		t->data.sdsset.data = new;
		break;
	default:
		break;
	}
}

void mem_gc(void)
{
// Remove tasks {{{
	mem_print_task_heap();
	//Task size, location, bytes to move
	uint16_t tsize, i, tomove;
	struct task *t = mem_task_head();
	while (t != NULL) {
	//	msg_log(SC("alloc_task %p %u mem_task: %u size: %u\n"),
	//		t, mem_rptr(t), mem_task, TASK_SIZE(t));
	//	msg_log(SC("id: %u\n"), t->taskid);
	//	msg_log(SC("stability: %u\n"), t->stability);
	//	msg_log(SC("returnwidth: %u\n"), t->returnwidth);
	//	msg_log(SC("bclen: %u\n"), t->bclen);
	//	msg_log(SC("sdslen: %u\n"), t->sdslen);
	//	msg_log(SC("peripherals: %u\n"), t->peripherals);
	//	msg_log(SC("tree: %u\n"), t->tree);

		if (t->tree == MT_REMOVE) {
			msg_log(SC("Task is removed, mem_task: %u, %p\n"),
				mem_task, t);
			//Size of the task
			tsize = TASK_SIZE(t);
			msg_log(SC("Size: %u\n"), tsize);

			//Location of the task
			i = mem_rptr(t);
			msg_log(SC("task loc: %p %u\n"), t, i);

			//Where to stop
			tomove = i;
			for (struct task *tt = mem_task_next(t); tt != NULL;
					tt = mem_task_next(tt))
				tomove += TASK_SIZE(tt);
			msg_log(SC("move up to %lu\n"), tomove);

			//Move the bytes
			for (; i<tomove; i++) {
				mtmem[i] = mtmem[i+tsize];
			}

			//Decrement the next task pointer
			mem_task -= tsize;
			msg_log(SC("task removed, %u bytes cleared\n"), tsize);
			msg_log(SC("new mem_task: %lu\n"), mem_task);
		}
		t = mem_task_next(t);
	}
	mem_print_task_heap();
//}}}

// Compact task trees {{{
	mem_print_heap();

	uint16_t walker, hole;
	walker = hole = memsize - sizeof(struct tasktree);
	while (walker >= mem_heap) {
		//Is trash, pass over but keep hole pointer valid
		if (mem_cast_tree(walker)->trash) {
			walker -= sizeof(struct tasktree);
		//Not trash and no hole
		} else if (walker == hole) {
			hole = walker -= sizeof(struct tasktree);
		//Not trash and a hole
		} else {
			msg_debug(SC("no trash and hole, move\n"));
			tasktree_print_node(mem_cast_tree(walker));
			msg_debug(SC("\nwalker: %u - hole: %u\n"),
				walker, hole);

			struct tasktree *t = mem_cast_tree(hole);
			//Move and correct rptrs
			t->ptr = mem_cast_tree(walker)->ptr;
			mem_node_move(t, mem_cast_tree(walker));

			//Update parent
			if (t->ptr < mem_heap) {
				msg_debug(SC("Parent is a task\n"));
				mem_cast_task(t->ptr)->tree = hole;
			} else {
				msg_debug(SC("Parent is a tasktree\n"));
				mem_update_ptr(
					mem_cast_tree(mem_cast_tree(hole)->ptr),
					walker, hole);
			}
			hole -= sizeof(struct tasktree);
			walker -= sizeof(struct tasktree);
		}
	}
	if (walker != hole) {
		msg_debug(SC("mem_heap: %u, walker: %u - hole: %u\n"),
			mem_heap, walker, hole);
		msg_debug(SC("Compacted %u bytes\n"), hole - walker);
		mem_heap = hole+sizeof(struct tasktree);
		msg_debug(SC("mem_heap: %u, walker: %u - hole: %u\n"),
			mem_heap, walker, hole);
	}
	mem_print_heap();
//}}}
}

void mem_mark_trash(uint16_t t)
{
	mem_cast_tree(t)->trash = true;
	mem_cast_tree(t)->ptr = MT_NULL;
}

void mem_mark_trash_deep(uint16_t t, uint16_t *stack)
{
	*stack++ = MT_REMOVE;
	*stack++ = t;
	while (*--stack != MT_REMOVE) {
		struct tasktree *ctree = mem_cast_tree(*stack);
		msg_debug(SC("mark trash: %u "), *stack);
		tasktree_print(ctree, 0);
		msg_debug(SC("\n"));
		switch (ctree->task_type) {
			case BCSTABLENODE:
				*stack++ = ctree->data.stablenode.next;
				break;
			case BCUNSTABLENODE:
				*stack++ = ctree->data.unstablenode.next;
				break;
			case BCSTEP:
				*stack++ = ctree->data.step.lhs;
				break;
			case BCSTEPSTABLE:
				*stack++ = ctree->data.steps.lhs;
				break;
			case BCSTEPUNSTABLE:
				*stack++ = ctree->data.stepu.lhs;
				break;
			case BCSEQSTABLE:
				*stack++ = ctree->data.seqs.lhs;
				*stack++ = ctree->data.seqs.rhs;
				break;
			case BCSEQUNSTABLE:
				*stack++ = ctree->data.sequ.lhs;
				*stack++ = ctree->data.sequ.rhs;
				break;
			case BCREPEAT:
				*stack++ = ctree->data.repeat.oldtree;
				if (ctree->data.repeat.tree != MT_NULL)
					*stack++ = ctree->data.repeat.tree;
				break;
			case BCTOR:
				*stack++ = ctree->data.tor.lhs;
				*stack++ = ctree->data.tor.rhs;
				break;
			case BCTAND:
				*stack++ = ctree->data.tand.lhs;
				*stack++ = ctree->data.tand.rhs;
				break;
			case BCSDSSET:
				*stack++ = ctree->data.sdsset.data;
				break;
			default:
				break;
		}
		ctree->trash = true;
	}
}
