#include "os/timer.h"
#include "os/co_task.h"
#include "bsp/bsp.h"

extern u64 g_task_ticks64;

static void timer_task_handler(void *);
static timer_t timer_head = {
	.prev = &timer_head,
	.next = &timer_head
};
static bool _timer_start = false;

void timer_post(timer_t *timer, u32 delay)
{
	timer_t *node;
	u64 time = g_task_ticks64 + (delay > 0 ? delay : 1);
	os_disable_irq();
	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;
	os_enable_irq();
	if (!_timer_start) {
		co_task_create(timer_task_handler, NULL, 384);
		_timer_start = true;
	}
}

void timer_cancel(timer_t *timer)
{
	os_disable_irq();
	if (timer->prev != NULL && timer->next != NULL) {
		timer->prev->next = timer->next;
		timer->next->prev = timer->prev;
	}
	timer->next = timer->prev = timer;
	os_enable_irq();
}

static void timer_task_handler(void *args)
{
	while (1) {
		timer_t *timer = timer_head.next;
#if 0
		if (timer->time > g_task_ticks64) {
			co_task_delay((u32)(timer->time - g_task_ticks64));
		}
#endif
		if ((timer->time <= g_task_ticks64) && (timer != &timer_head)) {
			timer_cancel(timer);
			timer->handler(timer);
		}
		co_task_yield();
	}
}