#include "libs/task.h"
#include "hal/hal.h"
#include "bsp/bsp.h"

static u64 task_mseconds;
static task_t _task_handler[MAX_TASK];
static u32 timer_task_handler(void);

static timer_t timer_head = {
	.prev = &timer_head,
	.next = &timer_head
};

static task_t task_head = {
	.next = &task_head,
	.handler = timer_task_handler
};

void system_tick_handler(void)
{
	task_mseconds++;
}

u64 get_mseconds(void)
{
	return task_mseconds;
}

u32 get_seconds(void){
	return task_mseconds/1000;
}

void task_ticks_enable(void)
{
	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}

u32 task_ticks_abs(void)
{
	return DWT->CYCCNT;
}

u32 task_ticks_rel(u32 start)
{
	u32 ticks = DWT->CYCCNT;

	if (ticks >= start) {
		return ticks - start;
	}

	return 0xFFFFFFFF - start + ticks + 1;
}

void task_ticks_delay(u32 ticks)
{
	u32 start;

	start = task_ticks_abs();
	while (task_ticks_rel(start) < ticks);
}

void task_udelay(u32 delay)
{
	task_ticks_delay(delay * (SystemCoreClock / 1000000));
}


static inline void timer_sync(void)
{
	task_head.time = timer_head.next->time;
}

void timer_post(timer_t *timer, u32 delay)
{
	timer_t *node;
	u64 time;

	__disable_irq();
	time = task_mseconds + (delay > 0 ? delay : 1);

	if (timer->prev != NULL) {
		timer->prev->next = timer->next;
	}

	if (timer->next != NULL) {
		timer->next->prev = timer->prev;
	}

	for (node = timer_head.next; node != &timer_head; node = node->next) {
		if (node->time > time) {
			break;
		}
	}

	timer->prev = node->prev;
	node->prev->next = timer;
	node->prev = timer;
	timer->next = node;
	timer->time = time;
	timer_sync();
	__enable_irq();
}

void timer_cancel(timer_t *timer)
{
	__disable_irq();
	if (timer->prev != NULL && timer->next != NULL) {
		timer->prev->next = timer->next;
		timer->next->prev = timer->prev;
	}
	timer->next = timer->prev = timer;
	timer_sync();
	__enable_irq();
}

static u32 timer_task_handler(void)
{
	while (1) {
		timer_t *timer = timer_head.next;

		if (timer->time > task_mseconds) {
			return timer->time - task_mseconds;
		}

		if (timer != &timer_head) {
			timer_cancel(timer);
			timer->handler(timer);
		} else {
			break;
		}

		wdog_reload();
	}

	return 0xFFFFFFFF;
}

void task_add(task_t *task)
{
	__disable_irq();

	task->next = task_head.next;
	task_head.next = task;

	__enable_irq();
}

task_t* task_start(task_func func, u32 delay){
	task_t *task = NULL;
	
	for (int i = 0; i < MAX_TASK; i++) {
		__disable_irq();
		if (_task_handler[i].handler == NULL) {
			task = _task_handler + i;
			task->handler = func;
			task->time = delay;
			task->next = task_head.next;
			task_head.next = task;	
			__enable_irq();
			return task;
		}
		__enable_irq();
	}	
	return NULL;
}


void task_run(void)
{
	task_t *head = &task_head;

	while (1) {
		wdog_reload();

		if (head->time <= task_mseconds) {
			head->time = task_mseconds + head->handler();
		}

		head = head->next;
	}
}