#include "libs/shark_task.h"
#include "bsp/shark_bsp.h"
#include "bsp/fmc_flash.h"
#include "app/iap.h"

extern uint32_t get_system_sleep_time(void);

static u64 shark_mseconds;
static u32 shark_timer_task_handler(void);

static shark_timer_t shark_timer_head = {
	.prev = &shark_timer_head,
	.next = &shark_timer_head
};

static shark_task_t shark_task_head = {
	.next = &shark_task_head,
	.handler = shark_timer_task_handler
};

void SysTick_Handler(void)
{
	shark_mseconds++;
}

u64 shark_get_mseconds(void)
{
	return shark_mseconds;
}

u32 shark_get_seconds(void){
	return shark_mseconds/1000 + get_system_sleep_time();
}

static inline void shark_timer_sync(void)
{
	shark_task_head.time = shark_timer_head.next->time;
}

void shark_timer_post(shark_timer_t *timer, u32 delay)
{
	shark_timer_t *node;
	u64 time;

	__disable_irq();

	time = shark_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 = shark_timer_head.next; node != &shark_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;
	shark_timer_sync();

	__enable_irq();
}

void shark_timer_cancel(shark_timer_t *timer)
{
	__disable_irq();

	timer->prev->next = timer->next;
	timer->next->prev = timer->prev;
	timer->next = timer->prev = timer;
	shark_timer_sync();

	__enable_irq();
}

static u32 shark_timer_task_handler(void)
{
	while (1) {
		shark_timer_t *timer = shark_timer_head.next;

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

		if (timer != &shark_timer_head) {
			shark_timer_cancel(timer);
			timer->handler(timer);
		} else {
			break;
		}

		wdog_reload();
	}

	return 0xFFFFFFFF;
}

void shark_task_add(shark_task_t *task)
{
	__disable_irq();

	task->next = shark_task_head.next;
	shark_task_head.next = task;

	__enable_irq();
}

void shark_task_run(void)
{
	shark_task_t *head = &shark_task_head;

	fmc_write_magic(0xFFFFFFFF, 0xFFFFFFFF, SHARK_IAP_MAGIC_SUCCESS);

	/* setup systick timer for 1000Hz interrupts */
	SysTick_Config(SystemCoreClock / 1000);
	/* configure the systick handler priority */
	NVIC_SetPriority(SysTick_IRQn, 0x00U);

	while (1) {
		wdog_reload();

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

		head = head->next;
	}
}