#include "task.h"

#include "communication_interface.h"
#include "mem.h"
#include "scheduler.h"
#include "events.h"

extern struct MTask *current_task;

void task_complete(struct MTask *t, uint16_t *stack)
{
	msg_debug(SC("Complete task: %s\n"),
		stack[0] == MTNoValue_c ? "No value"
		: stack[0] == MTUnstable_c ? "Unstable"
		: "Stable");

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

	//See if they are the same
	if (stack[0] != MTNoValue_c && t->value.size > 0)
		for (uint8_t i = 0; i<t->value.size/2 && same; i++)
			if (stack[i+1] != t->value.elements[i*2]*256u+
					t->value.elements[i*2+1]%256u)
				same = false;

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

	//Not the same or different stability
	if (!same) {
		msg_debug(SC("stability: %u\n"), stack[0]);
		struct TaskValue r;
		t->stability = stack[0];
		if (stack[0] == MTNoValue_c) {
			r.cons = NoValue_c;
		} else {
			r.cons = Value_c;
			//Copy it to the return space
			msg_debug(SC("return bytes: %lu\n"), t->value.size);
			msg_log(SC("send new task value\n"));
			for (uint8_t i = 0; i<t->value.size/2; i++) {
				t->value.elements[i*2] = stack[i+1] / 256;
				t->value.elements[i*2+1] = stack[i+1] % 256;
				msg_debug(SC("%u %u "),
					stack[i+1] / 256, stack[i+1] % 256);
			}
			msg_debug(SC("\n"));
			//Prepare the message
			r.data.Value.f0 = &t->value;
			r.data.Value.f1 = stack[0] == MTStable_c;
		}
		//Send the message
		send_message((struct MTMessageFro)
			{ .cons=MTFTaskReturn_c
			, .data={.MTFTaskReturn= { .f0=t->taskid, .f1=r }}
			});
	}
}

void task_remove(struct MTask *t, uint16_t *stack)
{
	if (t->tree != NULL)
		mem_mark_trash_and_destroy(t->tree, t, stack);
	t->stability = MTRemoved_c;
}

void destroy_task_node(struct TaskTree* tt, struct MTask *t)
{
	switch (tt->task_type) {
		case BCInterrupt_c:
			deregister_events(t, tt);
			break;
	}
}

void task_print(struct MTask *t, bool verbose)
{
	msg_debug(SC("register task allocated at %p\n"), t);
	//Server id
	msg_debug(SC("taskid: %u\n"), t->taskid);
	msg_debug(SC("tree: %p\n"), t->tree);
	msg_debug(SC("stability: %u\n"), t->stability);
	msg_debug(SC("value width in bytes: %u\n"), t->value.size);
	for (uint8_t i = 0; i<t->value.size && verbose; i++)
		msg_debug(SC("value[%u]=%02x\n"), i, t->value.elements[i]);
	msg_debug(SC("number of shares : %lu\n"), t->shares.size);
	for (uint8_t i = 0; i<t->shares.size && verbose; i++) {
		msg_debug(SC("share[%u]={id=%u, itask=%s, value=\n"), i,
			t->shares.elements[i].bcs_ident,
			t->shares.elements[i].bcs_itasks ? "yes" : "no");
		for (uint8_t j = 0; j<t->shares.elements[i].bcs_value.size; j++)
			msg_debug(SC("value[%u]=%u\n"), i,
				t->shares.elements[i].bcs_value.elements[j]);
		msg_debug(SC("}\n"));
	}
	msg_debug(SC("number of peripherals : %lu\n"), t->peripherals.size);
	msg_debug(SC("last run: %lu\n"), t->lastrun);
	msg_debug(SC("next run: [%lu, %lu]\n"), t->execution_min, t->execution_max);
	msg_debug(SC("number of instructions bytes: %lu\n"),
		t->instructions.size);
	for (uint16_t i = 0; i<t->instructions.size && verbose; i++)
		msg_debug(SC("bc[%u]=%u\n"), i, t->instructions.elements[i]);
}

bool task_register(struct MTask *data)
{
	task_print(data, true);

	//Pad bytecode so that the next task is aligned
#ifdef REQUIRE_ALIGNED_MEMORY_ACCESS
	uintptr_t d = PALIGND(PTRSIZE, mem_task_next_unsafe(data));
	data->instructions.size += d;
	extern uint16_t mem_task;
	mem_task += d;
#endif /* REQUIRE_ALIGNED_MEMORY_ACCESS */

#define CHECKP(i, s)\
	if ( (p->data.BCInitialised = i) == NULL ) {\
		msg_log(SC("Failed to setup " s "\n"));\
		send_message((struct MTMessageFro)\
			{ .cons=MTFException_c\
			, .data={.MTFException=\
				{ .cons=MTEUnsupportedPeripheral_c\
				, .data={.MTEUnsupportedPeripheral=\
					{ .f0=data->taskid\
					, .f1=p->cons\
					}\
				}}\
			}});\
		return false;\
	} else {\
		p->cons = BCInitialised_c;\
	}

	// Initialize peripherals
	for (uint32_t i = 0; i<data->peripherals.size; i++) {
		struct BCPeripheral *p = data->peripherals.elements+i;
		msg_log(SC("peripheral add: %u\n"), p->cons);
		switch (p->cons) {
#ifdef HAVE_DHT
		case BCDHT_c:
			CHECKP(dht_init(p->data.BCDHT), "DHT");
			break;
#endif /* HAVE_DHT */
#ifdef HAVE_LEDMATRIX
		case BCLEDMatrix_c:
			CHECKP(ledmatrix_init(p->data.BCLEDMatrix),
				"LEDMatrix");
			break;
#endif /* HAVE_LEDMATRIX */
#ifdef HAVE_I2CBUTTON
		case BCI2CButton_c:
			CHECKP(i2c_init(p->data.BCI2CButton), "I2CButtons");
			break;
#endif /* HAVE_I2CBUTTON */
#ifdef HAVE_LIGHTSENSOR
		case BCLightSensor_c:
			CHECKP(lightsensor_init(p->data.BCLightSensor),
				"Lightsensor");
			break;
#endif /* HAVE_LIGHTSENSOR */
#ifdef HAVE_AIRQUALITYSENSOR
		case BCAirQualitySensor_c:
			CHECKP(airqualitysensor_init(
				p->data.BCAirQualitySensor),
				"AirQualitySensor");
			break;
#endif /* HAVE_AIRQUALITYSENSOR */
#ifdef HAVE_GESTURESENSOR
		case BCGestureSensor_c:
			CHECKP(gesturesensor_init(p->data.BCGestureSensor),
				"GestureSensor");
			break;
#endif /* HAVE_GESTURESENSOR */
		default:
			CHECKP(NULL, "?? it is unsupported");
			break;
		}
	}

	// Set the last execution time of the task to now
	data->lastrun = getmillis();

	// Add task to the execution queue
	queue_insert_task(data);

	//Ack
	send_message((struct MTMessageFro)
		{ .cons=MTFTaskAck_c, .data={ .MTFTaskAck=data->taskid } });
	return true;
}

//TODO make non-recursive
void tasktree_print(struct TaskTree *t, int indent)
{
#if LOGLEVEL == 2
	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_c:
		msg_debug(SC("BCStablenode (%u): ("), t->data.stablenode.w);
		tasktree_print(t->data.stablenode.next, 0);
		msg_debug(SC(") %d %d"), t->data.stablenode.stable[0],
			t->data.stablenode.stable[1]);
		break;
	case BCStable0_c:
		msg_debug(SC("BCStable0"));
		break;
	case BCStable1_c:
		msg_debug(SC("BCStable1: %d"), t->data.stable[0]);
		break;
	case BCStable2_c:
		msg_debug(SC("BCStable2: %d %d"), t->data.stable[0],
			t->data.stable[1]);
		break;
	case BCStable3_c:
		msg_debug(SC("BCStable3: %d %d %d"), t->data.stable[0],
			t->data.stable[1], t->data.stable[2]);
		break;
	case BCStable4_c:
		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_c:
		msg_debug(SC("BCUnstablenode (%u): ("),
			t->data.unstablenode.w);
		tasktree_print(t->data.unstablenode.next, 0);
		msg_debug(SC(") %d %d"), t->data.unstablenode.unstable[0],
			t->data.unstablenode.unstable[1]);
		break;
	case BCUnstable0_c:
		msg_debug(SC("BCUnstable0"));
		break;
	case BCUnstable1_c:
		msg_debug(SC("BCUnstable1: %d"), t->data.unstable[0]);
		break;
	case BCUnstable2_c:
		msg_debug(SC("BCUnstable2: %d %d"), t->data.unstable[0],
			t->data.unstable[1]);
		break;
	case BCUnstable3_c:
		msg_debug(SC("BCUnstable3: %d %d %d"), t->data.unstable[0],
			t->data.unstable[1], t->data.unstable[2]);
		break;
	case BCUnstable4_c:
		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_c:
		msg_debug(SC("readD %d"), t->data.readd);
		break;
	case BCWriteD_c:
		msg_debug(SC("writeD %d %d"), t->data.writed.pin,
			t->data.writed.value);
		break;
	case BCReadA_c:
		msg_debug(SC("readA %d"), t->data.readd);
		break;
	case BCWriteA_c:
		msg_debug(SC("writeA %d %d"), t->data.writed.pin,
			t->data.writed.value);
		break;
	case BCPinMode_c:
		msg_debug(SC("pinmode %d %d"), t->data.pinmode.pin,
			t->data.pinmode.pinmode);
		break;
	case BCInterrupt_c:
		msg_debug(SC("interrupt %d %d"), t->data.interrupt.pin,
			t->data.interrupt.mode);
		break;
	case BCRepeat_c:
		msg_debug(SC("repeat ("));
		tasktree_print(t->data.repeat.tree, indent+1);
		msg_debug(SC(")"));
		break;
	case BCDelay_c:
		msg_debug(SC("delay (%u)"), t->data.delay);
		break;
	case BCDelayUntil_c:
		msg_debug(SC("delayuntil (%u)"), t->data.until);
		break;
	case BCStep_c:
		msg_debug(SC("("));
		tasktree_print(t->data.step.lhs, indent+1);
		msg_debug(SC(") >>* %lu"), t->data.step.rhs);
		break;
	case BCStepStable_c:
		msg_debug(SC("("));
		tasktree_print(t->data.steps.lhs, indent+1);
		msg_debug(SC(") >>= %lu"), t->data.steps.rhs);
		break;
	case BCStepUnstable_c:
		msg_debug(SC("("));
		tasktree_print(t->data.stepu.lhs, indent+1);
		msg_debug(SC(") >>~ %lu"), t->data.stepu.rhs);
		break;
	case BCSeqStable_c:
		msg_debug(SC("("));
		tasktree_print(t->data.seqs.lhs, indent+1);
		msg_debug(SC(") >>| "));
		tasktree_print(t->data.seqs.rhs, indent+1);
		msg_debug(SC(")"));
		break;
	case BCSeqUnstable_c:
		msg_debug(SC("("));
		tasktree_print(t->data.sequ.lhs, indent+1);
		msg_debug(SC(") >>. ("));
		tasktree_print(t->data.sequ.rhs, indent+1);
		msg_debug(SC(")"));
		break;
	case BCTOr_c:
		msg_debug(SC("("));
		tasktree_print(t->data.tor.lhs, indent+1);
		msg_debug(SC(") -||- ("));
		tasktree_print(t->data.tor.rhs, indent+1);
		msg_debug(SC(")"));
		break;
	case BCTAnd_c:
		msg_debug(SC("("));
		tasktree_print(t->data.tand.lhs, indent+1);
		msg_debug(SC(") -&&- ("));
		tasktree_print(t->data.tand.rhs, indent+1);
		msg_debug(SC(")"));
		break;
	case BCSdsSet_c:
		msg_debug(SC("setSDS %u"), t->data.sdsset.sds->bcs_ident);
		break;
	case BCSdsGet_c:
		msg_debug(SC("getSDS %u"), t->data.sdsget->bcs_ident);
		break;
	case BCRateLimit_c:
		msg_debug(SC("rateLimit ("));
		tasktree_print(t->data.ratelimit.task, indent+1);
		msg_debug(SC(")"));
		break;
	case BCSdsUpd_c:
		msg_debug(SC("updSDS %u"), t->data.sdsupd.sds->bcs_ident);
		break;
	}
#else /* LOGLEVEL == 2 */
	(void)t;
	(void)indent;
#endif /* LOGLEVEL == 2 */
}

void tasktree_print_node(struct TaskTree *t)
{
#if LOGLEVEL == 2
	switch (t->task_type) {
		case BCStable0_c:
		case BCStable1_c:
		case BCStable2_c:
		case BCStable3_c:
		case BCStable4_c:
			msg_debug(SC("Stable"));
			break;
		case BCStableNode_c:
			msg_debug(SC("StableNode %p"), t->data.stablenode.next);
			break;
		case BCUnstable0_c:
		case BCUnstable1_c:
		case BCUnstable2_c:
		case BCUnstable3_c:
		case BCUnstable4_c:
			msg_debug(SC("Unstable"));
			break;
		case BCUnstableNode_c:
			msg_debug(SC("UnstableNode %p"), t->data.unstablenode.next);
			break;
		case BCReadD_c:
			msg_debug(SC("Readd"));
			break;
		case BCWriteD_c:
			msg_debug(SC("Writed"));
			break;
		case BCReadA_c:
			msg_debug(SC("Reada"));
			break;
		case BCWriteA_c:
			msg_debug(SC("Writea"));
			break;
		case BCPinMode_c:
			msg_debug(SC("PinMode"));
			break;
		case BCInterrupt_c:
			msg_debug(SC("Interrupt"));
			break;
		case BCRepeat_c:
			msg_debug(SC("Repeat %p"), t->data.repeat.oldtree);
			break;
		case BCDelayUntil_c:
			msg_debug(SC("DelayUntil"));
			break;
		case BCDelay_c:
			msg_debug(SC("Delay"));
			break;
		case BCTAnd_c:
			msg_debug(SC("And %p %p"),
				t->data.tand.lhs, t->data.tand.rhs);
			break;
		case BCTOr_c:
			msg_debug(SC("Or %p %p"),
				t->data.tor.lhs, t->data.tor.rhs);
			break;
		case BCStep_c:
			msg_debug(SC("Step %p (%u, %u"),
				t->data.step.lhs, t->data.step.w, t->data.step.rhs);
			break;
		case BCStepStable_c:
			msg_debug(SC("StepStable %p (%u, %u)"),
				t->data.steps.lhs, t->data.steps.w, t->data.steps.rhs);
			break;
		case BCStepUnstable_c:
			msg_debug(SC("StepUnstable %p) (%u, %u)"),
				t->data.stepu.lhs, t->data.stepu.w, t->data.stepu.rhs);
			break;
		case BCSeqStable_c:
			msg_debug(SC("Seq stable %p %p"),
				t->data.seqs.lhs, t->data.seqs.rhs);
			break;
		case BCSeqUnstable_c:
			msg_debug(SC("Seq unstable %p %p"),
				t->data.sequ.lhs, t->data.sequ.rhs);
			break;
		case BCSdsGet_c:
			msg_debug(SC("Sdsget"));
			break;
		case BCSdsSet_c:
			msg_debug(SC("Sdsset"));
			break;
		case BCRateLimit_c:
			msg_debug(SC("RateLimit %p"), t->data.ratelimit.task);
			break;
		case BCSdsUpd_c:
			msg_debug(SC("Sdsupd"));
			break;
#ifdef HAVE_DHT
		case BCDHTTemp_c:
			msg_debug(SC("BCDhttemp"));
			break;
		case BCDHTHumid_c:
			msg_debug(SC("BCDhthumid"));
			break;
#endif /* HAVE_DHT */
#ifdef HAVE_AIRQUALITYSENSOR
		case BCSetEnvironmentalData_c:
			msg_debug(SC("BCSetEnvironmental"));
			break;
		case BCTVOC_c:
			msg_debug(SC("BCTVOC"));
			break;
		case BCCO2_c:
			msg_debug(SC("BCTCO2"));
			break;
#endif /* HAVE_AIRQUALITYSENSOR */
#ifdef HAVE_GESTURESENSOR
		case BCGesture_c:
			msg_debug(SC("BCGesture"));
			break;
#endif /* HAVE_GESTURESENSOR */
#ifdef HAVE_I2CBUTTON
		case BCAButton_c:
			msg_debug(SC("BCAButton"));
			break;
		case BCBButton_c:
			msg_debug(SC("BCBButton"));
			break;
#endif /* HAVE_I2CBUTTON */
#ifdef HAVE_LEDMATRIX
		case BCLEDMatrixDisplay_c:
			msg_debug(SC("BCLEDMatrixDisplay"));
			break;
		case BCLEDMatrixIntensity_c:
			msg_debug(SC("BCLEDMatrixIntensity %u"),
				t->data.ledmatrixintensity.intensity);
			break;
		case BCLEDMatrixDot_c:
			msg_debug(SC("BCLEDMatrixDot %u %u %u"),
				t->data.ledmatrixdot.x,
				t->data.ledmatrixdot.y,
				t->data.ledmatrixdot.s);
			break;
		case BCLEDMatrixClear_c:
			msg_debug(SC("BCLEDMatrixClear"));
			break;
#endif /* HAVE_LEDMATRIX */
		case BCEvent_c:
			msg_debug(SC("Event %u %u %u"),
				t->data.event.task_id,
				t->data.event.event_type,
				mem_rptr(t->data.event.next));
			break;
		default:
			break;
	}
#else /* LOGLEVEL == 2 */
	(void)t;
#endif /* LOGLEVEL == 2 */
}

struct TaskTree *tasktree_clone(struct TaskTree *treep, struct TaskTree *rptr)
{
	struct TaskTree *tree = treep;
	struct TaskTree *new = mem_alloc_tree();
	if (new == NULL)
		return NULL;
	*new = *tree;
	new->ptr = rptr;
	new->seconds = tree->seconds;
	new->refresh_min = tree->refresh_min;
	new->refresh_max = tree->refresh_max;

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

	switch (tree->task_type) {
	case BCStableNode_c:
		CLONE(new->data.stablenode.next, tree->data.stablenode.next);
		break;
	case BCUnstableNode_c:
		CLONE(new->data.unstablenode.next,
			tree->data.unstablenode.next);
		break;
	case BCRepeat_c:
		CLONE(new->data.repeat.tree, tree->data.repeat.tree);
		CLONE(new->data.repeat.oldtree, tree->data.repeat.oldtree);
		break;
	case BCTAnd_c:
		CLONE(new->data.tand.lhs, tree->data.tand.lhs);
		CLONE(new->data.tand.rhs, tree->data.tand.rhs);
		break;
	case BCTOr_c:
		CLONE(new->data.tor.lhs, tree->data.tor.lhs);
		CLONE(new->data.tor.rhs, tree->data.tor.rhs);
		break;
	case BCStep_c:
		CLONE(new->data.step.lhs, tree->data.step.lhs);
		break;
	case BCStepStable_c:
		CLONE(new->data.steps.lhs, tree->data.steps.lhs);
		break;
	case BCStepUnstable_c:
		CLONE(new->data.stepu.lhs, tree->data.stepu.lhs);
		break;
	case BCSeqStable_c:
		CLONE(new->data.seqs.lhs, tree->data.seqs.lhs);
		CLONE(new->data.seqs.rhs, tree->data.seqs.rhs);
		break;
	case BCSeqUnstable_c:
		CLONE(new->data.sequ.lhs, tree->data.sequ.lhs);
		CLONE(new->data.sequ.rhs, tree->data.sequ.rhs);
		break;
	case BCSdsSet_c:
		CLONE(new->data.sdsset.data, tree->data.sdsset.data);
		break;
	case BCRateLimit_c:
		CLONE(new->data.ratelimit.task, tree->data.ratelimit.task);
		CLONE(new->data.ratelimit.storage, tree->data.ratelimit.storage);
		new->data.ratelimit.last_execution = tree->data.ratelimit.last_execution;
		break;
	case BCSdsUpd_c:
		CLONE(new->data.sdsupd.ctx, tree->data.sdsupd.ctx);
		break;
	case BCDelay_c:
		new->data.delay = tree->data.delay;
		break;
	case BCDelayUntil_c:
		new->data.until = tree->data.until;
		break;
	default:
		break;
	}
	return new;
}

void *peripheral_get(uint8_t id)
{
	if (id >= current_task->peripherals.size) {
		send_message((struct MTMessageFro)
			{ .cons=MTFException_c
			, .data={.MTFException=
				{ .cons=MTEPeripheralUnknown_c
				, .data={.MTEPeripheralUnknown=
					{.f0=current_task->taskid, .f1=id}
				}}
			}});
		return NULL;
	}
	return current_task->peripherals.elements[id].data.BCInitialised;
}

struct TaskTree *create_result_task(uint16_t *sp, uint16_t *stack)
{
	if (stack == sp)
		return NULL;

	int left = stack - sp;
	struct TaskTree *t;
	struct TaskTree *next;

	// Select node type
	uint8_t node_type = BCUnstable0_c;
	node_type += min(left, 5);

	t = mem_alloc_tree();
	if (t == NULL) {
		msg_log(SC("error during tree allocation, resetting\n"));
		mem_reset();
	}

	t->task_type = node_type;
	t->trash = false;

	uint8_t capacity = 0;
	if (node_type == BCUnstableNode_c) {
		t->data.unstablenode.next = NULL;
		t->data.unstablenode.w = left - 2;
		t->data.unstablenode.unstable[0] = *sp++;
		t->data.unstablenode.unstable[1] = *sp++;

		next = create_result_task(sp, stack);

		next->ptr = t;
		t->data.unstablenode.next = next;
	} else {
		capacity = min(left, 4);
		for (int s = 0; s < capacity; s++) {
			t->data.unstable[s] = *sp++;
		}
	}

	return t;
}
