#include "os/os_task.h"
#include "os/os_types.h"
#include "bsp/bsp_driver.h"
#include "config.h"
#include "bsp/bsp.h"
extern uint32_t get_system_sleep_time(void);

static u64 shark_mseconds = 0;
static u32 _g_ticks = 0;
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++;
	_g_ticks ++;
}


u64 shark_get_mseconds(void)
{
	return shark_mseconds;
}

u32 shark_get_seconds(void){
	return _g_ticks/1000;
}

u32 get_tick_ms(void) {
	return _g_ticks;
}

u32 get_delta_ms(uint32_t prev_ms){
	uint32_t current = get_tick_ms();
	if (current >= prev_ms){
		return current - prev_ms;
	}
	//32λwrapper�����current < prev_ms
	return 0xFFFFFFFF - prev_ms + current;
}


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 *p)
{
	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();
}

shark_task_t *shark_task_create(TskFunction_t handler, void *params) {
	shark_task_t *ntsk = os_alloc(sizeof(shark_task_t));
	if (ntsk == NULL) {
		return ntsk;
	}
	ntsk->time = shark_mseconds;
	ntsk->handler = handler;
	ntsk->params = params;
	shark_task_add(ntsk);
	return ntsk;
}

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

	fmc_write_magic(IAP_MAGIC_SUCCESS);

	while (1) {
		wdog_reload();

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

		head = head->next;
	}
}