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

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

#define LOOPDELAY 5

struct task *current_task;

void read_message(void)
{
	bool waiting_for_task = false;

	//Find next task
	while (input_available() || waiting_for_task) {
		uint8_t b = read_byte();
		msg_log(SC("byte received: %02x\n"), b);
		if (b == 255) {
			msg_log(("Error or hangup, resetting\n"));
			reset();
			return;
		}
		switch (b) {
		case MTTTASKPREP:
			msg_log(SC("Got task prep\n"));
			write_byte(MTFTASKPREPACK);
			//Reply
			write_byte(read_byte());
			write_byte('\n');
			msg_log(SC("Waiting for the actual task\n"));
			waiting_for_task = true;
			break;
		case MTTTASK:
			msg_log(SC("Got task register\n"));
			waiting_for_task = false;
			task_register();
			break;
		case MTTTASKDEL:
			msg_log(SC("Delete task\n"));
			current_task = mem_task_head();
			b = read_byte();
			msg_log(SC("Delete task: %u\n"), b);
			while (current_task != NULL) {
				if (current_task->taskid == b) {
					task_remove(current_task, mem_stack());
					current_task = NULL;
				} else {
					current_task =
						mem_task_next(current_task);
				}
			}
			write_byte(MTFTASKDELACK);
			write_byte(b);
			write_byte('\n');
			break;
		case 'c':
		case MTTSPECREQUEST:
			msg_log(SC("Sending spec\n"));
			spec_send();
			break;
		case MTTSHUTDOWN:
			msg_log(SC("Got reset\n"));
			reset();
			break;
		case MTTSDSUPDATE:
			msg_log(SC("Got upstream sds update\n"));
			sds_recv();
			break;
		case '\n':
			break;
		default:
			msg_log(SC("Unknown byte\n"), b);
			break;
		}
	}
}

#ifdef ARDUINO
uint64_t nextrun = 0;
#endif
void real_loop(void)
{
#ifdef ARDUINO
	if (getmillis() > nextrun) {
		nextrun = getmillis()+LOOPDELAY;
	} else {
		msdelay(50);
		return;
	}
#endif

	real_yield();
	read_message();
	mem_gc();

	uint16_t sp = 0;
	uint16_t *stack = mem_stack();

	//Run tasks
	current_task = mem_task_head();
	while (current_task != NULL) {
		msg_debug(SC("\nExecute task %u, %p\n"), &current_task);
		if (current_task->tree == MT_NULL) {
			msg_log(SC("Execute main function, stack on %p\n"),
				mem_stack());

			//Run the main function and set the tasktree
			sp = 0;
			INITSTACKMAIN(stack, sp);
			if (interpret(TASK_BC(current_task), 0, sp, stack)) {
				current_task->tree = stack[0];

				msg_log(SC("returned on: %u which is at %p\n"),
					current_task->tree,
					mem_cast_tree(current_task->tree));
				tasktree_print(
					mem_cast_tree(current_task->tree), 0);
				msg_debug(SC("\n"));
			//Errored
			} else {
				msg_debug(SC("interpret errored, resetting\n"));
				mem_reset();
				break;
			}
		} else if (current_task->tree == MT_REMOVE) {
			msg_log(SC("Huh, this should've been garbage collected\n"));
		} else {
			msg_debug(SC("\n"));
			if (rewrite(mem_cast_tree(current_task->tree),
					TASK_BC(current_task), stack) != NULL) {
				msg_debug(SC("print sid: %u, tree: %u\n"),
					current_task->taskid,
					current_task->tree);
				tasktree_print(mem_cast_tree(
					current_task->tree), 0);
				task_complete(current_task, stack);
				if (stack[0] == MT_STABLE) {
					msg_log(SC(
						"Task is stable, removing\n"));
					task_remove(current_task, stack);
				}
			} else {
				msg_debug(SC("rewrite errored, resetting\n"));
				mem_reset();
				break;
			}
		}
		current_task = mem_task_next(current_task);
	}
}

void real_main()
{
	mem_reset();
	real_setup();
	RECOVERABLE_ERROR_CODE;

#if !defined(ARDUINO)
	while (true) {
		real_loop();
		msdelay(LOOPDELAY);
	}
#endif
}

#ifdef PC
int main(int argc, char *argv[])
{
	gargc = argc;
	gargv = argv;
	real_main();
	return 0;
}
#endif
