#include "client.h"

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

#include "interface.h"
#include "communication_interface.h"
#include "interpret.h"
#include "rewrite.h"
#include "spec.h"
#include "task.h"
#include "mem.h"
#include "scheduler.h"
#include "interrupts.h"
#include "config.h"

struct MTask *current_task;
uint32_t now;

static void read_message(void)
{
	uint8_t waiting_for_task = 0;

	//Find next task
	while (input_available() || waiting_for_task > 0) {
		void *taskptr = mem_stack();
		struct MTMessageTo msg = receive_message();
		bool ok;
		switch ((uint8_t)msg.cons) {
		case MTTTaskPrep_c:
			msg_log(SC("Got task prep for id: %u\n"),
				msg.data.MTTTaskPrep);
			send_message((struct MTMessageFro)
				{ .cons=MTFTaskPrepAck_c
				, .data={.MTFTaskPrepAck=msg.data.MTTTaskPrep}
				});
			msg_log(SC("Waiting for the actual task\n"));
			waiting_for_task++;
			break;
		case MTTTask_c:
			msg_log(SC("Got task register\n"));
			waiting_for_task--;
			ok = task_register(msg.data.MTTTask);
			if (! ok)
				mem_set_stack_ptr(taskptr);
			break;
		case MTTTaskDel_c:
			msg_log(SC("Delete task: %u\n"), msg.data.MTTTaskDel);

			// Remove from queue
			queue_remove_task(msg.data.MTTTaskDel);

			// Remove from memory
			current_task = mem_task_head();
			while (current_task != NULL) {
				if (current_task->taskid == msg.data.MTTTaskDel) {
					task_remove(current_task, mem_stack());
					current_task = NULL;
				} else {
					current_task =
						mem_task_next(current_task);
				}
			}
			send_message((struct MTMessageFro)
				{ .cons=MTFTaskDelAck_c
				, .data={ .MTFTaskDelAck=msg.data.MTTTaskDel }
				});
			mem_gc_tasks();
			break;
		case MTTSpecRequest_c:
			msg_log(SC("Sending spec\n"));
			spec_send();
			break;
		case MTTShutdown_c:
			msg_log(SC("Got shutdown\n"));
			reset();
			break;
		case MTTSdsUpdate_c:
			msg_log(SC("Got upstream sds update\n"));
			sds_update(msg.data.MTTSdsUpdate.f0
				, msg.data.MTTSdsUpdate.f1
				, msg.data.MTTSdsUpdate.f2);
			//Here we have to free the mallocs from the sds value
			mem_set_stack_ptr(taskptr);
			break;
		case 255:
			msg_log(SC("Got EOF\n"));
			reset();
			break;
		default:
			msg_log(SC("Unknown message: %d\n"), msg.cons);
			break;
		}
	}
}

static void wait(void)
{
	struct MTask *nexttask = peek_next_task();
	uint32_t nextrun;

	if (nexttask != NULL) {
		nextrun = nexttask->lastrun + nexttask->execution_max;
	} else {
		nextrun = now + DEFAULT_SLEEP_TIME;
	}

	/* sleep*/
	uint32_t sleep_time = 0;
	sleep_time = nextrun - getmillis();

	if (sleep_time > 0 && sleep_time <= INF_SLEEP_TIME &&
		!pending_interrupts()) {
		sleep_time = min(sleep_time, MAX_SLEEP_TIME);
		msg_log(SC("Sleep for %i ms\n"), sleep_time);
		mssleep(sleep_time);
	}
}

void real_loop(void)
{
	now = getmillis();

	real_yield();
	read_message();
	mem_gc();
	process_interrupts();
	uint16_t sp = 0;
	uint16_t *stack = mem_stack();
	bool removed = false;

	//Run tasks
	current_task = next_task(now);

	while (current_task != NULL) {
		msg_debug(SC("\nExecute task %u, %p\n"),
			current_task->taskid, &current_task);

		current_task->lastrun = now;

		if (current_task->stability == MTRemoved_c) {
			msg_log(SC("Huh, this should've been garbage collected\n"));
		} else if (current_task->tree == NULL) {
			msg_debug(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(current_task->instructions.elements,
					0, sp, stack)) {
				current_task->tree = mem_ptr(stack[0]);
				queue_insert_task(current_task);

				msg_debug(SC("returned on %p which is %u\n"),
					current_task->tree,
					mem_rptr(current_task->tree));
				tasktree_print(current_task->tree, 0);
				msg_debug(SC("\n"));
			//Errored
			} else {
				msg_log(SC("interpret errored, resetting\n"));
				mem_reset();
				break;
			}
		} else {
			msg_debug(SC("\n"));

			enum MTaskEvalStatus old_task_status = current_task->status;
			current_task->status = MTEvaluated_c;

			if (rewrite(current_task->tree,
					current_task->instructions.elements,
					stack) != NULL) {
				msg_debug(SC("print sid: %u, tree: %u\n"),
					current_task->taskid,
					current_task->tree);
				tasktree_print(current_task->tree, 0);
				task_complete(current_task, stack);
				if (stack[0] == MTStable_c) {
					msg_log(SC(
						"Task is stable, removing\n"));
					task_remove(current_task, stack);
					removed = true;
				} else {
					if (current_task->status == MTPurged_c ||
							current_task->status != old_task_status)
						calculate_execution_interval(current_task);
					queue_insert_task(current_task);
				}
			} else {
				msg_log(SC("rewrite errored, resetting\n"));
				mem_reset();
				break;
			}
		}
		current_task = next_task(now);
	}
	if (removed)
		mem_gc_tasks();

	//Sleep
	wait();
}

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

#if !defined(ARDUINO)
	while (true) {
		real_loop();
	}
#endif /* !defined(ARDUINO) */
}

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