#include "scheduler.h"

#include "mem.h"

static uint8_t queue_start = 0;
static uint8_t queue_end = 0;
static struct MTask *task_queue[TASK_QUEUE_SIZE];

// Refresh rate calculation
static void calculate_refresh_rate(struct TaskTree *tt, uint64_t lastrun,
	uint32_t *refresh_min, uint32_t *refresh_max)
{
	uint32_t refresh_min2;
	uint32_t refresh_max2;
	uint32_t multiplier = 1;

	if (tt->seconds) {
		multiplier = 1000;
	}

	if (tt->refresh_min <= tt->refresh_max && tt->task_type != BCRepeat_c &&
		tt->task_type != BCRateLimit_c) {
		*refresh_min = tt->refresh_min * multiplier;
		*refresh_max = tt->refresh_max * multiplier;
		return;
	}

	switch (tt->task_type) {
	//Repeat, delay and step {{{
	case BCRepeat_c:
		if (tt->data.repeat.tree == NULL) {
			*refresh_min = 0;
			*refresh_max = 0;
		} else if (tt->data.repeat.done) {
			*refresh_min = (tt->data.repeat.start +
					tt->refresh_min * multiplier) -
						lastrun;
			*refresh_max = (tt->data.repeat.start +
					tt->refresh_max * multiplier) -
						lastrun;
		} else {
			calculate_refresh_rate(tt->data.repeat.tree, lastrun,
				refresh_min, refresh_max);
		}
		break;
	case BCStep_c:
		calculate_refresh_rate(tt->data.step.lhs, lastrun, refresh_min,
			refresh_max);
		break;
	case BCStepStable_c:
		calculate_refresh_rate(tt->data.steps.lhs, lastrun, refresh_min,
			refresh_max);
		break;
	case BCSeqStable_c:
		calculate_refresh_rate(tt->data.seqs.lhs, lastrun, refresh_min,
			refresh_max);
		break;
	case BCStepUnstable_c:
		calculate_refresh_rate(tt->data.stepu.lhs, lastrun, refresh_min,
			refresh_max);
		break;
	case BCSeqUnstable_c:
		calculate_refresh_rate(tt->data.sequ.lhs, lastrun, refresh_min,
			refresh_max);
		break;
	case BCDelayUntil_c:
		if (lastrun < tt->data.until) {
			uint32_t delay = tt->data.until - lastrun;
			*refresh_min = delay;
			*refresh_max = delay;
		}
		break;
		//}}}
		//Parallel {{{
	case BCTOr_c:
	case BCTAnd_c:
		calculate_refresh_rate(tt->data.tor.lhs, lastrun, refresh_min,
						refresh_max);
		calculate_refresh_rate(tt->data.tor.rhs, lastrun, &refresh_min2,
						&refresh_max2);
		if (*refresh_min > refresh_max2) {
			*refresh_max = refresh_max2;
			*refresh_min = refresh_min2;
		} else if (refresh_min2 > *refresh_max) {
			// refresh min and max already have the correct value
		} else {
			*refresh_min = max(*refresh_min, refresh_min2);
			*refresh_max = min(*refresh_max, refresh_max2);
		}

		break;
		//}}}
		//Sds {{{
	case BCSdsGet_c:
		*refresh_min = 0;
		*refresh_max = 2000;
		break;
		//}}}
		//Rate limit {{{
	case BCRateLimit_c:
		*refresh_min = (tt->data.ratelimit.last_execution +
			tt->refresh_min * multiplier) -
				lastrun;
		*refresh_max = (tt->data.ratelimit.last_execution +
			tt->refresh_max * multiplier) -
				lastrun;
		break;
		//}}}
		//Pin IO {{{
	case BCReadD_c:
	case BCReadA_c:
		*refresh_min = 0;
		*refresh_max = 100;
		break;
		//}}}
		// Interrupts {{{
	case BCInterrupt_c:
		*refresh_min = INF_SLEEP_TIME;
		*refresh_max = INF_SLEEP_TIME;
		break;
		//}}}
		//Peripherals {{{
		//DHT {{{
#ifdef HAVE_DHT
	case BCDHTTemp_c:
	case BCDHTHumid_c:
		*refresh_min = 0;
		*refresh_max = 2000;
		break;
#endif /* HAVE_DHT */
		//}}}
		//I2CButton {{{
#ifdef HAVE_I2CBUTTON
	case BCAButton_c:
	case BCBButton_c:
		*refresh_min = 0;
		*refresh_max = 100;
		break;
#endif /* HAVE_I2CBUTTON */
		//}}}
		//LIGHTSENSOR {{{
#ifdef HAVE_LIGHTSENSOR
	case BCGetLight_c:
		*refresh_min = 0;
		*refresh_max = 100;
		break;
#endif /* HAVE_LIGHTSENSOR */
		//}}}
		//AIRQUALITYSENSOR {{{
#ifdef HAVE_AIRQUALITYSENSOR
	case BCTVOC_c:
	case BCCO2_c:
		*refresh_min = 0;
		*refresh_max = 2000;
		break;
#endif /* HAVE_AIRQUALITYSENSOR */
		//}}}
		//GESTURESENSOR {{{
#ifdef HAVE_GESTURESENSOR
	case BCGesture_c:
		*refresh_min = 0;
		*refresh_max = 1000;
		break;
#endif /* HAVE_GESTURESENSOR */
		//}}}
		//AIRQUALITYSENSOR {{{
#ifdef HAVE_AIRQUALITYSENSOR
	case BCSetEnvironmentalData_c:
#endif /* HAVE_AIRQUALITYSENSOR */
		//}}}
		//LEDMatrix {{{
#ifdef HAVE_LEDMATRIX
	case BCLEDMatrixDisplay_c:
	case BCLEDMatrixIntensity_c:
	case BCLEDMatrixDot_c:
	case BCLEDMatrixClear_c:
#endif /* HAVE_LEDMATRIX */
		//}}}
		//}}}
		//Constant node values {{{
	case BCStableNode_c:
	case BCStable0_c:
	case BCStable1_c:
	case BCStable2_c:
	case BCStable3_c:
	case BCStable4_c:
		*refresh_min = INF_SLEEP_TIME;
		*refresh_max = INF_SLEEP_TIME;
		break;
		//}}}
	default:
		*refresh_min = 0;
		*refresh_max = 0;
		break;
		//}}}
	}
}

void calculate_execution_interval(struct MTask *task)
{
	uint32_t refresh_min = 0;
	uint32_t refresh_max = 0;

	if (task->status != MTUnevaluated_c) {
		calculate_refresh_rate(task->tree, task->lastrun, &refresh_min,
			&refresh_max);
	}

	task->execution_min = min(refresh_min, INF_SLEEP_TIME);
	task->execution_max = min(refresh_max, INF_SLEEP_TIME);

	msg_debug(SC("execution interval: [%u, %u] \n"), refresh_min,
		refresh_max);
}

// Task queue
static void print_queue()
{
#if LOGLEVEL == 2
	uint8_t i = queue_start;
	msg_debug(SC("task queue: ["));
	while (i != queue_end) {
		if (i != queue_start)
			msg_debug(SC(", "));

		msg_debug(SC("%u"), task_queue[i]->taskid);
		i = (i + 1) % TASK_QUEUE_SIZE;
	}
	msg_debug(SC("] \n"));
#endif /* LOGLEVEL == 2 */
}

struct MTask *peek_next_task()
{
	if (queue_start == queue_end)
		return NULL;

	return task_queue[queue_start];
}

struct MTask *next_task(uint32_t now)
{
	struct MTask *out = peek_next_task();

	// Stop condition
	if (out == NULL || out->lastrun == now ||
		now - (out->lastrun + out->execution_min) >= INF_SLEEP_TIME) {
		return NULL;
	}

	queue_start = (uint8_t)(queue_start + 1) % TASK_QUEUE_SIZE;

	print_queue();
	return out;
}
void queue_insert_task(struct MTask *t)
{
	msg_debug(SC("insert task: %u\n"),
		t->taskid, queue_start, queue_end);

	uint8_t prev_end = queue_end;
	queue_end = (uint8_t)(queue_end + 1) % TASK_QUEUE_SIZE;
	if (queue_end == queue_start) {
		msg_log(SC("task queue full, resetting\n"));
		mem_reset();
	}

	uint32_t rr = t->lastrun + t->execution_max;
	uint8_t i = queue_start;
	bool found = false;

	while (i != queue_end) {
		struct MTask *cur = task_queue[i];

		if (!found &&
		    (i == prev_end ||
		     rr - (cur->lastrun + cur->execution_max) > INF_SLEEP_TIME)) {
			found = true;
		}

		if (found) {
			task_queue[i] = t;
			t = cur;
		}

		i = (i + 1) % TASK_QUEUE_SIZE;
	}

	print_queue();
}

void queue_remove_task(uint8_t id)
{
	if (queue_start == queue_end) {
		return;
	}

	uint8_t i = queue_start;
	struct MTask *cur;
	struct MTask *prev = NULL;
	bool found = false;

	while (i != queue_end) {
		cur = task_queue[i];

		task_queue[i] = prev;

		if (cur->taskid == id) {
			found = true;
			break;
		}

		prev = cur;
		i = (i + 1) % TASK_QUEUE_SIZE;
	}

	if (!found) {
		queue_end = (uint8_t)(queue_end + 1) % TASK_QUEUE_SIZE;
		task_queue[i] = prev;
	}

	queue_start = (uint8_t)(queue_start + 1) % TASK_QUEUE_SIZE;

	print_queue();
}

void queue_update_pointer(struct MTask *old, struct MTask *new) {
	uint8_t i = queue_start;
	while (i != queue_end) {
		if (task_queue[i] == old) {
			task_queue[i] = new;
			return;
		}
		i = (i + 1) % TASK_QUEUE_SIZE;
	}
}
