#include <stdio.h>
#include <stdlib.h>

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

extern struct task *current_task;

//TODO make non-recursive
uint16_t *rewrite(struct tasktree *t, uint8_t *program, uint16_t *stack)
{
	msg_debug(SC("rewriting code stack: %p\n"), stack);
	uint8_t *b;
	uint16_t *oldstack;
	uint16_t width;
	float tfloat;

//	for (uint16_t j = 0; j<sp+2; j++)
//		msg_debug(SC("stack[%u]: %u\n"), j, stack[j]);

	switch (t->task_type) {
//Constant node values {{{
	case BCSTABLENODE:
		*stack++ = MT_STABLE;
		*stack++ = t->data.stablenode.stable[0];
		*stack++ = t->data.stablenode.stable[1];
		oldstack = stack;
		stack = rewrite(
			mem_cast_tree(t->data.stablenode.next), program, stack);
		if (stack == NULL)
			return NULL;

		for ( ; oldstack < stack; oldstack++)
			oldstack[0] = oldstack[1];
		stack--;
		break;
	case BCSTABLE0:
		msg_debug(SC("Rewrite:Stable0\n"));
		*stack++ = MT_STABLE;
		break;
	case BCSTABLE1:
		msg_debug(SC("Rewrite:Stable1\n"));
		*stack++ = MT_STABLE;
		*stack++ = t->data.stable[0];
		break;
	case BCSTABLE2:
		msg_debug(SC("Rewrite:Stable2\n"));
		*stack++ = MT_STABLE;
		*stack++ = t->data.stable[0];
		*stack++ = t->data.stable[1];
		break;
	case BCSTABLE3:
		msg_debug(SC("Rewrite:Stable3\n"));
		*stack++ = MT_STABLE;
		*stack++ = t->data.stable[0];
		*stack++ = t->data.stable[1];
		*stack++ = t->data.stable[2];
		break;
	case BCSTABLE4:
		msg_debug(SC("Rewrite:Stable4\n"));
		*stack++ = MT_STABLE;
		*stack++ = t->data.stable[0];
		*stack++ = t->data.stable[1];
		*stack++ = t->data.stable[2];
		*stack++ = t->data.stable[3];
		break;
	case BCUNSTABLENODE:
		msg_debug(SC("Rewrite:Unstablenode\n"));
		*stack++ = MT_UNSTABLE;
		*stack++ = t->data.unstablenode.unstable[0];
		*stack++ = t->data.unstablenode.unstable[1];
		oldstack = stack;
		stack = rewrite(
			mem_cast_tree(t->data.stablenode.next), program, stack);
		if (stack == NULL)
			return NULL;

		for ( ; oldstack < stack; oldstack++)
			oldstack[0] = oldstack[1];
		stack--;
		break;
	case BCUNSTABLE0:
		msg_debug(SC("Rewrite:Unstable0\n"));
		*stack++ = MT_UNSTABLE;
		break;
	case BCUNSTABLE1:
		msg_debug(SC("Rewrite:Unstable1\n"));
		*stack++ = MT_UNSTABLE;
		*stack++ = t->data.unstable[0];
		break;
	case BCUNSTABLE2:
		msg_debug(SC("Rewrite:Unstable2\n"));
		*stack++ = MT_UNSTABLE;
		*stack++ = t->data.unstable[0];
		*stack++ = t->data.unstable[1];
		break;
	case BCUNSTABLE3:
		msg_debug(SC("Rewrite:Unstable3\n"));
		*stack++ = MT_UNSTABLE;
		*stack++ = t->data.unstable[0];
		*stack++ = t->data.unstable[1];
		*stack++ = t->data.unstable[2];
		break;
	case BCUNSTABLE4:
		msg_debug(SC("Rewrite:Unstable4\n"));
		*stack++ = MT_UNSTABLE;
		*stack++ = t->data.unstable[0];
		*stack++ = t->data.unstable[1];
		*stack++ = t->data.unstable[2];
		*stack++ = t->data.unstable[3];
		break;
//}}}
//Pin IO {{{
	case BCREADD:
		msg_debug(SC("Rewrite:ReadD\n"));
		*stack++ = MT_UNSTABLE;
		*stack++ = read_dpin(t->data.readd);
		break;
	case BCWRITED:
		msg_debug(SC("Rewrite:WriteD\n"));
		write_dpin(t->data.writed.pin, t->data.writed.value);

		*stack++ = MT_STABLE;
		*stack++ = t->data.writed.value;

		t->task_type = BCSTABLE1;
		t->data.stable[0] = t->data.writed.value;
		break;
	case BCREADA:
		msg_debug(SC("Rewrite:ReadA\n"));
		*stack++ = MT_UNSTABLE;
		*stack++ = read_apin(t->data.reada);
		break;
	case BCWRITEA:
		msg_debug(SC("Rewrite:WriteA\n"));
		write_apin(t->data.writea.pin, t->data.writea.value);

		*stack++ = MT_STABLE;
		*stack++ = t->data.writea.value;

		t->task_type = BCSTABLE1;
		t->data.stable[0] = t->data.writea.value;
		break;
	case BCPINMODE:
		msg_debug(SC("Rewrite:pinmode\n"));
		set_pinmode(t->data.pinmode.pin, t->data.pinmode.pinmode);
		t->task_type = BCSTABLE0;
		*stack++ = MT_STABLE;
		break;
//}}}
//Repeat, delay and step {{{
	case BCREPEAT:
		msg_debug(SC("Rewrite:Repeat\n"));
		if (t->data.repeat.tree == MT_NULL) {
			msg_debug(SC("Clone repeat tree\n"));
			t->data.repeat.tree =
				tasktree_clone(t->data.repeat.oldtree,
					mem_rptr(t));
			if (t->data.repeat.tree == MT_NULL)
				return NULL;
			tasktree_print(mem_cast_tree(t->data.repeat.tree), 0);
			msg_debug(SC("\n"));
		}
		//Stable, thus generate the task tree again
		if (rewrite(mem_cast_tree(t->data.repeat.tree),
				program, stack) == NULL)
			return NULL;

		//Stable, thus recalculate
		if (*stack == MT_STABLE) {
			//Clean up lhs
			mem_mark_trash_deep(t->data.repeat.tree, stack);
			t->data.repeat.tree = MT_NULL;
		}

		*stack = MT_NOVALUE;
		break;
	case BCSTEP:
		msg_debug(SC("Rewrite:Step (%u)\n"), t->data.step.w);
		tasktree_print(t, 0);
		msg_debug(SC("\n"));
		//Save the stack pointer
		oldstack = stack;
		//Prepare the stack for interpretation
		width = 0;
		INITSTACKMAIN(stack, width);
		//Rewrite
		stack = rewrite(
			mem_cast_tree(t->data.step.lhs), program, stack+width);
		if (stack == NULL)
			return NULL;
		for (uint16_t *sp = oldstack; sp != stack; sp++) {
			msg_debug("sp: %lu\n", *sp);
		}
		//Interpret
		if (!interpret(program, t->data.step.rhs,
				stack-oldstack, oldstack))
			return NULL;
		for (uint16_t *sp = oldstack; sp != stack; sp++) {
			msg_debug("sp: %lu\n", *sp);
		}
		//Save the width, it might be destroyed if we had a match
		width = t->data.step.w;
		//There was no match...
		if (*oldstack == MT_NULL) {
			msg_debug(SC("Step nomatch\n"));
		} else {
			msg_debug(SC("Step match: %u\n"), *oldstack);
			tasktree_print(t, 0);
			msg_debug(SC("\nmatch\n"));
			tasktree_print(mem_cast_tree(*oldstack), 0);
			msg_debug(SC("\n"));

			//Clean up lhs
			mem_mark_trash_deep(t->data.step.lhs, stack);

			//Move the node to t
			mem_node_move(t, mem_cast_tree(*oldstack));

			msg_debug(SC("After move: %u\n"), *oldstack);
			tasktree_print(t, 0);
		}
		//Set the stack back to the old stack
		stack = oldstack;
		//Step never returns
		*stack++ = MT_NOVALUE;
		//pad with the rhs width
		stack += width;
		break;
	case BCSTEPSTABLE:
		msg_debug(SC("Rewrite:Stepstable (%u, %u)\n"),
			t->data.steps.w, t->data.steps.rhs);
		//Save the stack pointer
		oldstack = stack;
		//Prepare the stack for interpretation
		width = 0;
		INITSTACKMAIN(stack, width);
		//Rewrite
		stack = rewrite(
			mem_cast_tree(t->data.steps.lhs), program, stack+width);
		if (stack == NULL)
			return NULL;
		width = t->data.steps.w;
		//Width might be lost if we need to rewrite
		if (*(oldstack + STACKMETA) == MT_STABLE) {
			//Interpret the rhs
			if (!interpret(program, t->data.steps.rhs,
					stack-oldstack, oldstack))
				return NULL;

			//Clean up lhs
			mem_mark_trash_deep(t->data.steps.lhs, stack);

			//Move the node to t
			mem_node_move(t, mem_cast_tree(*oldstack));
		}
		//Set the stack back to the old stack
		stack = oldstack;
		//Step never returns
		*stack++ = MT_NOVALUE;
		//pad with the rhs width
		stack += width;
		break;
	case BCSTEPUNSTABLE:
		msg_debug(SC("Rewrite:Stepunstable (%u)\n"), t->data.stepu.w);
		oldstack = stack;
		width = 0;
		INITSTACKMAIN(stack, width);
		stack = rewrite(
			mem_cast_tree(t->data.stepu.lhs), program, stack+width);
		if (stack == NULL)
			return NULL;
		width = t->data.stepu.w;
		if (*(oldstack + STACKMETA) != MT_NOVALUE) {
			if (!interpret(program, t->data.stepu.rhs,
				stack-oldstack, oldstack))
				return NULL;
			mem_mark_trash_deep(t->data.stepu.lhs, stack);
			mem_node_move(t, mem_cast_tree(*oldstack));
		}
		stack = oldstack;
		*stack++ = MT_NOVALUE;
		stack += width;
		break;
	case BCSEQSTABLE:
		msg_debug(SC("Rewrite:seqs (%u)\n"), t->data.seqs.w);
		//Rewrite lhs
		if (rewrite(mem_cast_tree(t->data.seqs.lhs),
				program, stack) == NULL)
			return NULL;

		//Save the width, it might be destroyed if we had a match
		width = t->data.seqs.w;
		//There was no match...
		if (*stack != MT_STABLE) {
			msg_debug(SC("Step nomatch\n"));
		} else {
			msg_debug(SC("Step match\n"));

			//Clean up lhs
			mem_mark_trash_deep(t->data.seqs.lhs, stack);

			//Move the node to t
			mem_node_move(t, mem_cast_tree(t->data.seqs.rhs));
		}

		//Step never returns
		*stack++ = MT_NOVALUE;
		//pad with the rhs width
		stack += width;
		break;
	case BCSEQUNSTABLE:
		msg_debug(SC("Rewrite:sequ (%u)\n"), t->data.sequ.w);
		//Rewrite lhs
		if (rewrite(mem_cast_tree(t->data.sequ.lhs),
				program, stack) == NULL)
			return NULL;

		//Save the width, it might be destroyed if we had a match
		width = t->data.sequ.w;
		//There was no match...
		if (*stack == MT_NOVALUE) {
			msg_debug(SC("Step nomatch\n"));
		} else {
			msg_debug(SC("Step match\n"));

			//Clean up lhs
			mem_mark_trash_deep(t->data.sequ.lhs, stack);

			//Move the node to t
			mem_node_move(t, mem_cast_tree(t->data.sequ.rhs));
		}

		//Step never returns
		*stack++ = MT_NOVALUE;
		//pad with the rhs width
		stack += width;
		break;
	case BCDELAY:
		t->data.until = getmillis() + t->data.delay;
		t->task_type = BCDELAYUNTIL;
		/*-fallthrough*/
	case BCDELAYUNTIL:
		msg_debug(SC("Rewrite:DelayUntil\n"));
		msg_debug(SC("now: %lu\n"), getmillis());
		msg_debug(SC("until: %lu\n"), t->data.until);
		if (getmillis() > t->data.until) {
			*stack++ = MT_STABLE;
		} else {
			*stack++ = MT_UNSTABLE;
		}
		*stack++ = getmillis()-t->data.until;
		break;
//}}}
//Parallel {{{
	case BCTOR:
		oldstack = stack;
		stack = rewrite(mem_cast_tree(t->data.tor.lhs), program, stack);
		if (stack == NULL)
			return NULL;
		//msg_debug(SC("rewrite: sp: %u\n"), sp);
		//msg_debug(SC("After rewriting lhs\n"));
		//for (uint16_t j = 0; j<sp+2; j++)
		//	msg_debug(SC("stack[%u]: %u\n"), j, stack[j]);

		//lhs is stable, done
		if (*oldstack == MT_STABLE) {
			msg_debug(SC("lhs is stable, done\n"));
			//Clean up rhs
			mem_mark_trash_deep(t->data.tor.rhs, stack);
			//Move the lhs to t
			mem_node_move(t, mem_cast_tree(t->data.tor.lhs));
			break;
		//lhs has no value, use rhs
		} else if (*oldstack == MT_NOVALUE) {
			msg_debug(SC("lhs has no value, just eval rhs\n"));
			stack = oldstack;
			if (rewrite(mem_cast_tree(t->data.tor.rhs),
					program, stack) == NULL)
				return NULL;
		//lhs is unstable, eval rhs and see if it is more stable
		} else {
			/*
			 * oldstack-width : lhs stability
			 *                : lhs
			 * oldstack       : rhs stability
			 *                : rhs
			 * stack
			 */
			width = stack-oldstack;
			oldstack = stack;
			stack = rewrite(
				mem_cast_tree(t->data.tor.rhs), program, stack);
			if (stack == NULL)
				return NULL;
			if (*oldstack == MT_STABLE) {
				oldstack -= width;
				stack -= width;
				for (uint8_t i = 0; i<width; i++)
					oldstack[i] = stack[i];
				//Clean up lhs
				mem_mark_trash_deep(t->data.tor.lhs, stack);
				//Move the rhs to t
				mem_node_move(
					t, mem_cast_tree(t->data.tor.rhs));
			}
		}
		break;
	case BCTAND:
		msg_debug(SC("Rewrite .&&.\n"));

		//Rewrite the lhs and the rhs
		oldstack = stack;
		stack = rewrite(
			mem_cast_tree(t->data.tand.lhs), program, stack);
		if (stack == NULL)
			return NULL;
		msg_debug(SC("Rewritten lhs\n"));
		width = stack-oldstack;
		//rhs starts just right of lhs compensating for the stability
		oldstack = stack;
		stack = rewrite(
			mem_cast_tree(t->data.tand.rhs), program, stack);
		if (stack == NULL)
			return NULL;
		msg_debug(SC("Rewritten rhs\n"));

		/*
		 * oldstack-width : lhs stability
		 *                : lhs
		 * oldstack       : rhs stability
		 *                : rhs
		 * stack
		 */

		//The stability is the minimum of the both values
		msg_debug(SC("lhs stability: %u, rhs: %u\n"),
			*(oldstack-width), *oldstack);
		*(oldstack-width)
			= *(oldstack-width) < *oldstack
			? *(oldstack-width)
			: *oldstack;
		//Compress the taskvalue
		for (uint8_t i = 0; i<stack-oldstack; i++)
			oldstack[i] = oldstack[i+1];
		stack--;
		break;
//}}}
//Sds {{{
	case BCSDSGET:
		*stack++ = MT_UNSTABLE;
		msg_debug(SC("Get sds at %lu\n"), t->data.sdsget.sds);
		b = mem_cast_bytes(t->data.sdsget.sds);
		for (uint8_t i = 0; i<b[2]/2; i++)
			*stack++ = b[3+i*2]*256 + b[3+i*2+1];
		break;
	case BCSDSSET:
		msg_debug(SC("Get sds at %lu\n"), t->data.sdsset.sds);
		if (t->data.sdsset.sds == MT_NULL) {
			write_byte(MTFEXCEPTION);
			write_byte(MTESDSUNKNOWN);
			write_byte(current_task->taskid);
			write_byte(t->data.sdsset.sdsid);
			write_byte('\n');
			return NULL;
		}

		tasktree_print(mem_cast_tree(t->data.sdsset.data), 0);
		msg_debug(SC("\n"));
		oldstack = stack;
		stack = rewrite(
			mem_cast_tree(t->data.sdsset.data), program, stack);
		if (stack == NULL)
			return NULL;
		//Jump over the stability
		oldstack++;

		b = mem_cast_bytes(t->data.sdsset.sds);
		bool same = true;
		for (uint16_t j = 0; j<b[2]/2; j++) {
			same = same &&
				(b[3+j*2]*256u + b[3+j*2+1] == oldstack[j]);
			b[3+j*2] = oldstack[j]/256;
			b[3+j*2+1] = oldstack[j]%256;
		}

		//Send if it is different and an upstream sds
		msg_debug(SC("addr: %lu, itasks: %u, same: %u\n"),
			t->data.sdsset.sds, b[1], same);
		if (b[1] && !same) {
			msg_log(SC("send sds update\n"));
			write_byte(MTFSDSUPDATE);
			write_byte(current_task->taskid);
			write_byte(t->data.sdsset.sdsid);
			write_byte(b[2]);
			msg_log(SC("where b[2] == %u\n"), b[2]);
			for (uint16_t j = 0; j<b[2]/2; j++)
				write16bit(oldstack[j]);
			write_byte('\n');
		}

		//Save the position of the task
		stack[0] = t->data.sdsset.data;

		//Set this node to be the data
		*t = *mem_cast_tree(t->data.sdsset.data);

		//Clean up the data node
		mem_cast_tree(stack[0])->trash = true;

		break;
//}}}
//Peripherals {{{
//DHT {{{
#ifdef HAVE_DHT
	case BCDHTTEMP:
		msg_debug(SC("dht read temp: %u\n"), t->data.dhttemp);
		*stack++ = MT_UNSTABLE;
		tfloat = get_dht_temp(task_peripheral(t->data.dhttemp));
		*stack++ = float2uint32_t(tfloat) >> 16;
		*stack++ = float2uint32_t(tfloat) & 0xffff;
		break;
	case BCDHTHUMID:
		msg_debug(SC("dht read humid: %u\n"), t->data.dhthumid);
		*stack++ = MT_UNSTABLE;
		tfloat = get_dht_humidity(task_peripheral(t->data.dhthumid));
		*stack++ = float2uint32_t(tfloat) >> 16;
		*stack++ = float2uint32_t(tfloat) & 0xffff;
		break;
#endif
//}}}
//I2CButton {{{
#ifdef HAVE_I2CBUTTON
	case BCABUTTON:
		msg_debug(SC("abutton: %u\n"), t->data.abutton);
		*stack++ = MT_UNSTABLE;
		*stack++ = i2c_abutton(task_peripheral(t->data.abutton));
		break;
	case BCBBUTTON:
		msg_debug(SC("bbutton: %u\n"), t->data.bbutton);
		*stack++ = MT_UNSTABLE;
		*stack++ = i2c_bbutton(task_peripheral(t->data.bbutton));
		break;
#endif
//}}}
//LIGHTSENSOR {{{
#ifdef HAVE_LIGHTSENSOR
	case BCGETLIGHT:
		msg_debug(SC("light read: %u\n"), t->data.lightsensor);
		*stack++ = MT_UNSTABLE;
		tfloat = get_light(task_peripheral(t->data.lightsensor));
		*stack++ = float2uint32_t(tfloat) >> 16;
		*stack++ = float2uint32_t(tfloat) & 0xffff;
		break;
#endif
//}}}
//AIRQUALITYSENSOR {{{
#ifdef HAVE_AIRQUALITYSENSOR
	case BCSETENVIRONMENTALDATA:
//		b = task_peripheral(BCAIRQUALITYSENSOR,
//			t->data.setenvironment.sensor);
		msg_debug(SC("set environmental data: %u\n"),
				t->data.setenvironment.sensor);
		set_environmental_data(
				task_peripheral(t->data.setenvironment.sensor),
				t->data.setenvironment.humid,
				t->data.setenvironment.temp);
		t->task_type = BCSTABLE0;
		*stack++ = MT_STABLE;
		break;
	case BCTVOC:
//		b = task_peripheral(BCAIRQUALITYSENSOR,
//			t->data.airqualitysensor);
		msg_debug(SC("tvoc: %u\n"), t->data.airqualitysensor);
		*stack++ = MT_UNSTABLE;
		*stack++ = get_tvoc(task_peripheral(t->data.airqualitysensor));
		break;
	case BCCO2:
//		b = task_peripheral(BCAIRQUALITYSENSOR,
//			t->data.airqualitysensor);
		msg_debug(SC("co2: %u\n"), t->data.airqualitysensor);
		*stack++ = MT_UNSTABLE;
		*stack++ = get_co2(task_peripheral(t->data.airqualitysensor));
		break;
#endif
//}}}
//LEDMatrix {{{
#ifdef HAVE_LEDMATRIX
	case BCLEDMATRIXDISPLAY:
		msg_debug(SC("ledmatrixdisplay: %u\n"),
			t->data.ledmatrixdisplay);
		ledmatrix_display(task_peripheral(t->data.ledmatrixdisplay));
		*stack++ = MT_STABLE;
		break;
	case BCLEDMATRIXINTENSITY:
		msg_debug(SC("ledmatrixintensity: %u\n"),
			t->data.ledmatrixintensity.intensity);
		ledmatrix_intensity(
			task_peripheral(t->data.ledmatrixintensity.id),
			t->data.ledmatrixintensity.intensity);
		*stack++ = MT_STABLE;
		break;
	case BCLEDMATRIXDOT:
		msg_debug(SC("ledmatrixdot: %u %u %u\n"),
			t->data.ledmatrixdot.x,
			t->data.ledmatrixdot.y,
			t->data.ledmatrixdot.s);
		ledmatrix_dot(task_peripheral(t->data.ledmatrixdot.id),
			t->data.ledmatrixdot.x,
			t->data.ledmatrixdot.y,
			t->data.ledmatrixdot.s);
		*stack++ = MT_STABLE;
		break;
	case BCLEDMATRIXCLEAR:
		msg_debug(SC("ledmatrixclear: %u\n"), t->data.ledmatrixclear);
		ledmatrix_clear(task_peripheral(t->data.ledmatrixclear));
		*stack++ = MT_STABLE;
		break;
#endif
//}}}
//}}}
	}
	return stack;
}
