#include "os/co_task.h"

typedef enum {
	Task_Pending,
	Task_Blocking,
	Task_Ready,
	Task_Running
}Task_Stat_t;
typedef struct _tcb{
	volatile u32 fp;
	volatile u32 *pstack; //stack addr
	u32 start_tm_ms;
	Task_Stat_t  stat; //task stat, running, pending, blocked
	struct _tcb *next;
}tcb_t;

volatile tcb_t *current_tcb;

static tcb_t *g_tcb_head;
static volatile u32 _context_swap_cnt = 0;
static u32 g_task_ticks;

extern void arch_save_context(void);
extern void arch_restore_context(void);
extern void arch_start_first_task(void);
extern u32 arch_stack_init(volatile u32 *stack_base, u16 stack_size, void *fptr, void *p_arg);

static tcb_t *_next_task(void);
static void _task_add(tcb_t *tcb);
static void _task_context_swap(tcb_t *prev, tcb_t *next);
static void sys_tick_init(void);

void *co_task_create(task_func func, void *args, u16 stack_size){
	tcb_t *p_tcb = co_malloc(sizeof(tcb_t));
	p_tcb->pstack = (u32 *)co_malloc(stack_size);
	p_tcb->fp = arch_stack_init(p_tcb->pstack, stack_size>>2, func, args);
	p_tcb->start_tm_ms = 0;
	p_tcb->stat = Task_Ready;
	p_tcb->next = p_tcb;
	_task_add(p_tcb);
	return p_tcb;
}

void co_task_schedule(void){
	sys_tick_init();
	current_tcb = g_tcb_head;
	arch_start_first_task();
	while(1);
}

void co_task_yield(void){
	tcb_t *next = _next_task();
	if (next != NULL && next != current_tcb) {
		_task_context_swap((tcb_t *)current_tcb, next);
	}
}

void co_task_delay(int ms){
	current_tcb->start_tm_ms = ms + g_task_ticks;
	while(1) {
		tcb_t *next = _next_task();
		if (next != NULL){
			if (next == current_tcb) {
				break;
			}
			_task_context_swap((tcb_t *)current_tcb, next);
			break;
		}else {
			co_task_yield();
		}
	}
}

/* peak next task to run */
static tcb_t *_next_task(void) {
	tcb_t *next = current_tcb->next;
	for (; next != current_tcb; next = next->next) {
		if (next->stat != Task_Ready) {
			continue;
		}
		if (next->start_tm_ms <= g_task_ticks) {
			return next;
		}
	}
	if (current_tcb->start_tm_ms <= g_task_ticks) {
		return (tcb_t *)current_tcb;
	}	
	return NULL;
}

/* add task to running list */
static void _task_add(tcb_t *tcb) {
	if (g_tcb_head == NULL) {
		g_tcb_head = tcb;
		return;
	}
	tcb_t *next = g_tcb_head->next;
	//get the last one
	for (; next->next != g_tcb_head; next = next->next);
	tcb->next = next->next;
	next->next = tcb;
}

static void _task_context_swap(tcb_t *prev, tcb_t *next) {
	u32 prev_cnt = ++_context_swap_cnt;
	arch_save_context();
	if (prev_cnt == _context_swap_cnt) {
		current_tcb = next;
		arch_restore_context();
	}
}

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


static void task_ticks(void) {
	g_task_ticks ++;
}

void SysTick_Handler(void) {
	task_ticks();
}

extern void *pvPortMalloc(u32);
extern void vPortFree(void *);
void *co_malloc(u32 size) {
	return pvPortMalloc(size);
}

void co_free(void *ptr) {
	vPortFree(ptr);
}