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

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

#include "symbols.h"
#include "symbols_peripherals.h"

extern struct task *current_task;

void task_complete(struct task *t, uint16_t *stack)
{
	msg_debug(SC("Complete task: has value: %s\n"),
		stack[0] == MT_NOVALUE ? "No" : "Yes");
	msg_debug(SC("rw: %u\n"), t->returnwidth);
	uint16_t *ret = TASK_RET(t);

	//stability is equal
	bool same = stack[0] == t->stability;

	//See if they are the same
	if (stack[0] != MT_NOVALUE && t->returnwidth > 0)
		for (uint8_t i = 0; i<t->returnwidth && same; i++)
			if (stack[i+1] != ret[i])
				same = false;

	msg_debug(SC("and was %sthe same\n"), same ? "" : "not ");

	//Not the same or different stability
	if (!same) {
		write_byte(MTFTASKRETURN);
		write_byte(t->taskid);
		msg_log(SC("stability: %u\n"), stack[0]);
		write_byte(t->stability = stack[0]);
		if (t->stability != MT_NOVALUE) {
			write_byte(t->returnwidth*2);
			msg_log(SC("return bytes: %lu\n"), t->returnwidth*2);
			for (uint8_t i = 0; i<t->returnwidth; i++) {
				ret[i] = stack[i+1];
				msg_log(SC("ret: %u %u\n"),
					stack[i+1] / 256, stack[i+1] % 256);
				write_byte(stack[i+1] / 256);
				write_byte(stack[i+1] % 256);
			}
		}
		write_byte('\n');
	}
}

void task_remove(struct task *t, uint16_t *stack)
{
	if (t->tree != MT_NULL) {
		mem_mark_trash_deep(t->tree, stack);
	}
	t->tree = MT_REMOVE;
}

void task_register(void)
{
	msg_debug(SC("register\n"));
	//Server id
	uint8_t serverid = read_byte();
	msg_debug(SC("sid: %u\n"), serverid);

	//Return width
	uint8_t returnwidth = read_byte();
	msg_debug(SC("rw : %u\n"), returnwidth);

	//Number of bytes taken for the shares
	uint16_t sdslen = read16bit();
	msg_debug(SC("sdslen: %u\n"), sdslen);

	//Width of the peripherals
	uint8_t peripherals = read_byte();
	msg_debug(SC("peripherals: %u\n"), peripherals);

	//Number of instructions
	uint16_t bclen = read16bit();
	msg_debug(SC("bc : %u\n"), bclen);

	struct task *t = mem_alloc_task(
		serverid, returnwidth, sdslen, bclen,
		peripherals);
	if (t == NULL) {
		//Gobble up the rest of the input
		for (uint16_t i = 0; i<sdslen;) {
			read_byte();
			read_byte();
			returnwidth = read_byte();
			for (uint8_t j = 0; j<returnwidth; j++)
				read_byte();
			i += 3 + returnwidth;
		}
		for (uint8_t i = 0; i<peripherals; i++) {
			parsestruct_mt_BCPeripheral(read_byte, NULL);
		}
		for (uint16_t i = 0; i<bclen; i++)
			read_byte();
		mem_reset();
		return;
	}
	t->taskid = serverid;
	t->stability = MT_NOVALUE;
	t->tree = MT_NULL;
	msg_log(SC("sid: %u\n"), t->taskid);
	msg_debug(SC("rw : %u\n"), t->returnwidth);
	msg_debug(SC("bc : %u\n"), t->bclen);
	msg_debug(SC("per: %u\n"), t->peripherals);

	//Initialize return value
	uint8_t *bc = (uint8_t *)TASK_RET(t);
	for (uint8_t i = 0; i<t->returnwidth*2; i++) {
		bc[i] = 0;
	}

	bc = TASK_SDS(t);

	//Read SDSs
	msg_debug(SC("sds bytes to read: %d\n"), sdslen);
	for (uint16_t i = 0; i<sdslen;) {
		bc[i] = read_byte();
		msg_debug(SC("sdsid: %u\n"), bc[i]);
		bc[i+1] = read_byte();
		msg_debug(SC("iTasks ref: %u\n"), bc[i+1]);
		bc[i+2] = read_byte();
		msg_debug(SC("sdsbytes: %u\n"), bc[i+2]);
		for (uint8_t j = 0; j<bc[i+2]; j++) {
			bc[i+3+j] = read_byte();
			msg_debug(SC("sds[%u]: %02x\n"), j, bc[i+3+j]);
		}
		i += 3 + bc[i+2];
//		bc = bc + i;
	}

	//Read peripherals
#define PINIT(init, type, msg) {\
	p = (uintptr_t)(init);\
	if (p == 0) {\
		msg_log(SC("Error setting up: %s\n"), msg);\
		write_byte(MTFEXCEPTION);\
		write_byte(MTEUNSUPPORTEDPERIPHERAL);\
		write_byte(t->taskid);\
		write_byte(type);\
		write_byte('\n');\
		unknown_peripheral = true;\
	}\
	STOREPTR(bc+i*PTRSIZE, p);\
}

	bc = TASK_PER(t);
	bool unknown_peripheral = false;
	for (uint8_t i = 0; i<peripherals; i++) {
		struct mt_BCPeripheral r = parsestruct_mt_BCPeripheral(read_byte, NULL);
		if (unknown_peripheral)
			continue;
		uintptr_t p;
		msg_log(SC("peripheral added: %u\n"), r.cons);
		switch (r.cons) {
#ifdef HAVE_DHT
		case mt_BCPeripheral_BCDHT:
			PINIT(dht_init(r.data.mt_BCDHT),
				mt_BCPeripheral_BCDHT,
				"DHT not supported");
			break;
#endif
#ifdef HAVE_LEDMATRIX
		case mt_BCPeripheral_BCLEDMatrix:
			PINIT(ledmatrix_init(r.data.mt_BCLEDMatrix),
				mt_BCPeripheral_BCLEDMatrix,
				"LEDMatrix not supported");
			break;
#endif
#ifdef HAVE_I2CBUTTON
		case mt_BCPeripheral_BCI2CButton:
			PINIT(i2c_init(r.data.mt_BCI2CButton),
				mt_BCPeripheral_BCI2CButton,
				"I2CButtons not supported");
			break;
#endif
#ifdef HAVE_LIGHTSENSOR
		case BCLIGHTSENSOR:
			PINIT(lightsensor_init(r.data.mt_BCLightSensor),
				mt_BCPeripheral_BCLightSensor,
				"Lightsensor not supported");
			break;
#endif
#ifdef HAVE_AIRQUALITYSENSOR
		case BCAIRQUALITYSENSOR:
			PINIT(airqualitysensor_init(
					r.data.mt_BCAirQualitySensor),
				mt_BCPeripheral_BCAirQualitySensor,
				"AirQualitySensor not supported");
			break;
#endif
		default:
			PINIT(NULL, r.cons, "Unknown peripheral");
			break;
		}
	}
	if (unknown_peripheral) {
		t->tree = MT_REMOVE;
		for (uint16_t i = 0; i<bclen; i++)
			read_byte();
		return;
	}

	//Read BC
	msg_debug(SC("bc to read: %d\n"), bclen);
	bc = TASK_BC(t);
	for (uint16_t i = 0; i<bclen; i++) {
		bc[i] = read_byte();
		msg_debug(SC("%u %p - bc[%d]: %02x\n"),
			mem_rptr(bc+i), bc + i, i, bc[i]);
	}

	//Ack
	write_byte(MTFTASKACK);
	write_byte(t->taskid);
	write_byte('\n');
}

void *task_peripheral(uint8_t index)
{
	msg_debug(SC("get peripheral with idx: %u\n"), index);
	return GETPTR(TASK_PER(current_task) + index*PTRSIZE);
}

//TODO make non-recursive
#ifdef DEBUG
void tasktree_print_node(struct tasktree *t)
{
	switch (t->task_type) {
		case BCSTABLE0:
		case BCSTABLE1:
		case BCSTABLE2:
		case BCSTABLE3:
		case BCSTABLE4:
			msg_debug(SC("Stable"));
			break;
		case BCSTABLENODE:
			msg_debug(SC("StableNode %d"), t->data.stablenode.next);
			break;
		case BCUNSTABLE0:
		case BCUNSTABLE1:
		case BCUNSTABLE2:
		case BCUNSTABLE3:
		case BCUNSTABLE4:
			msg_debug(SC("Unstable"));
			break;
		case BCUNSTABLENODE:
			msg_debug(SC("Unstable %d"), t->data.unstablenode.next);
			break;
		case BCREADD:
			msg_debug(SC("Readd"));
			break;
		case BCWRITED:
			msg_debug(SC("Writed"));
			break;
		case BCREADA:
			msg_debug(SC("Reada"));
			break;
		case BCWRITEA:
			msg_debug(SC("Writea"));
			break;
		case BCPINMODE:
			msg_debug(SC("PinMode"));
			break;
		case BCREPEAT:
			msg_debug(SC("Repeat %d"), t->data.repeat.oldtree);
			break;
		case BCDELAYUNTIL:
			msg_debug(SC("DelayUntil"));
			break;
		case BCDELAY:
			msg_debug(SC("Delay"));
			break;
		case BCTAND:
			msg_debug(SC("And %d %d"),
				t->data.tand.lhs, t->data.tand.rhs);
			break;
		case BCTOR:
			msg_debug(SC("Or %d %d"),
				t->data.tor.lhs, t->data.tor.rhs);
			break;
		case BCSTEP:
			msg_debug(SC("Step %d (%u, %u"),
				t->data.step.lhs, t->data.step.w, t->data.step.rhs);
			break;
		case BCSTEPSTABLE:
			msg_debug(SC("StepStable %d (%u, %u)"),
				t->data.steps.lhs, t->data.steps.w, t->data.steps.rhs);
			break;
		case BCSTEPUNSTABLE:
			msg_debug(SC("StepUnstable %d (%u, %u)"),
				t->data.stepu.lhs, t->data.stepu.w, t->data.stepu.rhs);
			break;
		case BCSEQSTABLE:
			msg_debug(SC("Seq stable %d %d"),
				t->data.seqs.lhs, t->data.seqs.rhs);
			break;
		case BCSEQUNSTABLE:
			msg_debug(SC("Seq unstable %d %d"),
				t->data.sequ.lhs, t->data.sequ.rhs);
			break;
		case BCSDSGET:
			msg_debug(SC("Sdsget"));
			break;
		case BCSDSSET:
			msg_debug(SC("Sdsset"));
			break;
#ifdef HAVE_DHT
		case BCDHTTEMP:
			msg_debug(SC("BCDhttemp"));
			break;
		case BCDHTHUMID:
			msg_debug(SC("BCDhthumid"));
			break;
#endif
#ifdef HAVE_AIRQUALITYSENSOR
		case BCSETENVIRONMENTALDATA:
			msg_debug(SC("BCSetEnvironmental"));
			break;
		case BCTVOC:
			msg_debug(SC("BCTVOC"));
			break;
		case BCCO2:
			msg_debug(SC("BCTCO2"));
			break;
#endif
#ifdef HAVE_I2CBUTTON
		case BCABUTTON:
			msg_debug(SC("BCAButton"));
			break;
		case BCBBUTTON:
			msg_debug(SC("BCBButton"));
			break;
#endif
#ifdef HAVE_LEDMATRIX
		case BCLEDMATRIXDISPLAY:
			msg_debug(SC("BCLEDMatrixDisplay"));
			break;
		case BCLEDMATRIXINTENSITY:
			msg_debug(SC("BCLEDMatrixIntensity %u"),
				t->data.ledmatrixintensity.intensity);
			break;
		case BCLEDMATRIXDOT:
			msg_debug(SC("BCLEDMatrixDot %u %u %u"),
				t->data.ledmatrixdot.x,
				t->data.ledmatrixdot.y,
				t->data.ledmatrixdot.s);
			break;
		case BCLEDMATRIXCLEAR:
			msg_debug(SC("BCLEDMatrixClear"));
			break;
#endif
		default:
			break;
	}
}

void tasktree_print(struct tasktree *t, int indent)
{
	if (t == NULL) {
		msg_debug(SC("(null)\n"));
		return;
	}

	if (t->trash) {
		msg_debug(SC("(removed)\n"));
	}

//	for (int i = 0; i<indent; i++)
//		msg_debug(SC("\t"));

	switch (t->task_type) {
	case BCSTABLENODE:
		msg_debug(SC("BCStablenode (%u): ("), t->data.stablenode.w);
		tasktree_print(mem_cast_tree(t->data.stablenode.next), 0);
		msg_debug(SC(") %d %d"), t->data.stablenode.stable[0],
			t->data.stablenode.stable[1]);
		break;
	case BCSTABLE0:
		msg_debug(SC("BCStable0"));
		break;
	case BCSTABLE1:
		msg_debug(SC("BCStable1: %d"), t->data.stable[0]);
		break;
	case BCSTABLE2:
		msg_debug(SC("BCStable2: %d %d"), t->data.stable[0],
			t->data.stable[1]);
		break;
	case BCSTABLE3:
		msg_debug(SC("BCStable3: %d %d %d"), t->data.stable[0],
			t->data.stable[1], t->data.stable[2]);
		break;
	case BCSTABLE4:
		msg_debug(SC("BCStable4: %d %d %d %d"), t->data.stable[0],
			t->data.stable[1], t->data.stable[2],
			t->data.stable[3]);
		break;
	case BCUNSTABLENODE:
		msg_debug(SC("BCUnstablenode (%u): ("),
			t->data.unstablenode.w);
		tasktree_print(mem_cast_tree(t->data.unstablenode.next), 0);
		msg_debug(SC(") %d %d"), t->data.unstablenode.unstable[0],
			t->data.unstablenode.unstable[1]);
		break;
	case BCUNSTABLE0:
		msg_debug(SC("BCUnstable0"));
		break;
	case BCUNSTABLE1:
		msg_debug(SC("BCUnstable1: %d"), t->data.unstable[0]);
		break;
	case BCUNSTABLE2:
		msg_debug(SC("BCUnstable2: %d %d"), t->data.unstable[0],
			t->data.unstable[1]);
		break;
	case BCUNSTABLE3:
		msg_debug(SC("BCUnstable3: %d %d %d"), t->data.unstable[0],
			t->data.unstable[1], t->data.unstable[2]);
		break;
	case BCUNSTABLE4:
		msg_debug(SC("BCUnstable4: %d %d %d %d"),
			t->data.unstable[0], t->data.unstable[1],
			t->data.unstable[2], t->data.unstable[3]);
		break;
	case BCREADD:
		msg_debug(SC("readD %d"), t->data.readd);
		break;
	case BCWRITED:
		msg_debug(SC("writeD %d %d"), t->data.writed.pin,
			t->data.writed.value);
		break;
	case BCREADA:
		msg_debug(SC("readA %d"), t->data.readd);
		break;
	case BCWRITEA:
		msg_debug(SC("writeA %d %d"), t->data.writed.pin,
			t->data.writed.value);
		break;
	case BCPINMODE:
		msg_debug(SC("pinmode %d %d"), t->data.pinmode.pin,
			t->data.pinmode.pinmode);
		break;
	case BCREPEAT:
		msg_debug(SC("repeat ("));
		tasktree_print(mem_cast_tree(t->data.repeat.tree), indent+1);
		msg_debug(SC(")"));
		break;
	case BCDELAY:
		msg_debug(SC("delay (%u)"), t->data.delay);
		break;
	case BCDELAYUNTIL:
		msg_debug(SC("delayuntil (%u)"), t->data.until);
		break;
	case BCSTEP:
		msg_debug(SC("("));
		tasktree_print(mem_cast_tree(t->data.step.lhs), indent+1);
		msg_debug(SC(") >>* %lu"), t->data.step.rhs);
		break;
	case BCSTEPSTABLE:
		msg_debug(SC("("));
		tasktree_print(mem_cast_tree(t->data.steps.lhs), indent+1);
		msg_debug(SC(") >>= %lu"), t->data.steps.rhs);
		break;
	case BCSTEPUNSTABLE:
		msg_debug(SC("("));
		tasktree_print(mem_cast_tree(t->data.stepu.lhs), indent+1);
		msg_debug(SC(") >>~ %lu"), t->data.stepu.rhs);
		break;
	case BCSEQSTABLE:
		msg_debug(SC("("));
		tasktree_print(mem_cast_tree(t->data.seqs.lhs), indent+1);
		msg_debug(SC(") >>| "));
		tasktree_print(mem_cast_tree(t->data.seqs.rhs), indent+1);
		msg_debug(SC(")"));
		break;
	case BCSEQUNSTABLE:
		msg_debug(SC("("));
		tasktree_print(mem_cast_tree(t->data.sequ.lhs), indent+1);
		msg_debug(SC(") >>. ("));
		tasktree_print(mem_cast_tree(t->data.sequ.rhs), indent+1);
		msg_debug(SC(")"));
		break;
	case BCTOR:
		msg_debug(SC("("));
		tasktree_print(mem_cast_tree(t->data.tor.lhs), indent+1);
		msg_debug(SC(") -||- ("));
		tasktree_print(mem_cast_tree(t->data.tor.rhs), indent+1);
		msg_debug(SC(")"));
		break;
	case BCTAND:
		msg_debug(SC("("));
		tasktree_print(mem_cast_tree(t->data.tand.lhs), indent+1);
		msg_debug(SC(") -&&- ("));
		tasktree_print(mem_cast_tree(t->data.tand.rhs), indent+1);
		msg_debug(SC(")"));
		break;
	case BCSDSSET:
		msg_debug(SC("setSDS %u"), t->data.sdsset.sdsid);
		break;
	case BCSDSGET:
		msg_debug(SC("getSDS %u"), t->data.sdsget.sdsid);
		break;
	}
}
#endif

uint16_t tasktree_clone(uint16_t treep, uint16_t rptr)
{
	struct tasktree *tree = mem_cast_tree(treep);
	struct tasktree *new = mem_alloc_tree();
	if (new == NULL)
		return MT_NULL;
	uint16_t newp = mem_rptr(new);
	*new = *tree;
	new->ptr = rptr;

//Safe clone
#define CLONE(to, source) {\
	to = tasktree_clone(source, newp);\
	if (to == MT_NULL)\
		return MT_NULL;\
	}

	switch (tree->task_type) {
	case BCSTABLENODE:
		CLONE(new->data.stablenode.next, tree->data.stablenode.next);
		break;
	case BCUNSTABLENODE:
		CLONE(new->data.unstablenode.next,
			tree->data.unstablenode.next);
		break;
	case BCREPEAT:
		CLONE(new->data.repeat.tree, tree->data.repeat.tree);
		CLONE(new->data.repeat.oldtree, tree->data.repeat.oldtree);
		break;
	case BCTAND:
		CLONE(new->data.tand.lhs, tree->data.tand.lhs);
		CLONE(new->data.tand.rhs, tree->data.tand.rhs);
		break;
	case BCTOR:
		CLONE(new->data.tor.lhs, tree->data.tor.lhs);
		CLONE(new->data.tor.rhs, tree->data.tor.rhs);
		break;
	case BCSTEP:
		CLONE(new->data.step.lhs, tree->data.step.lhs);
		break;
	case BCSTEPSTABLE:
		CLONE(new->data.steps.lhs, tree->data.steps.lhs);
		break;
	case BCSTEPUNSTABLE:
		CLONE(new->data.stepu.lhs, tree->data.stepu.lhs);
		break;
	case BCSEQSTABLE:
		CLONE(new->data.seqs.lhs, tree->data.seqs.lhs);
		CLONE(new->data.seqs.rhs, tree->data.seqs.rhs);
		break;
	case BCSEQUNSTABLE:
		CLONE(new->data.sequ.lhs, tree->data.sequ.lhs);
		CLONE(new->data.sequ.rhs, tree->data.sequ.rhs);
		break;
	case BCSDSSET:
		CLONE(new->data.sdsset.data, tree->data.sdsset.data);
		break;
	case BCDELAY:
		new->data.delay = tree->data.delay;
		break;
	case BCDELAYUNTIL:
		new->data.until = tree->data.until;
		break;
	default:
		break;
	}
	return newp;
}

uint16_t sds_get_addr(uint8_t id)
{
	uint8_t *b = TASK_SDS(current_task);
	msg_debug(SC("Looking for %u\n"), id);
	msg_debug(SC("SDS stored at: %p\n"), b);
	for (uint16_t i = 0; i<current_task->sdslen;) {
		msg_debug(SC("SDS: id=%u, itask=%u, len=%u)\n"),
			b[i], b[i+1], b[i+2]);
		if (b[i] == id) {
			msg_debug(SC("Match\n"));
			return mem_rptr(b+i);
		} else {
			i += 3 + b[i+2];
		}
	}
	write_byte(MTFEXCEPTION);
	write_byte(MTESDSUNKNOWN);
	write_byte(current_task->taskid);
	write_byte(id);
	write_byte('\n');
	return MT_NULL;
}

void sds_recv(void)
{
	msg_debug(SC("Receiving an sds\n"));
	uint8_t taskid = read_byte();
	uint8_t sdsid = read_byte();
	uint8_t bytes = read_byte();
	msg_debug(SC("for task %u and sds %u\n"), taskid, sdsid);

	current_task = mem_task_head();
	while (current_task != NULL) {
		if (current_task->taskid == taskid) {
			msg_debug(SC("Found task for the sds update\n"));
			uint8_t *b = mem_cast_bytes(sds_get_addr(sdsid));
			if (b == NULL)
				break;

			for (uint8_t i = 0; i<bytes; i++) {
				b[3+i] = read_byte();
			}
			return;
		}
		msg_debug(SC("\nExecute task %u\n"), current_task->taskid);
	}

	msg_debug(SC("Help, received an unknown sds\n"));

	//Skip over the rest of the message then
	for (uint8_t i = 0; i<bytes; i++) {
		read_byte();
	}
}
