#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          stack_size;
	u32 start_tm_ms;
	Task_Stat_t  stat; //task stat, running, pending, blocked
	struct _tcb *next;
}tcb_t;

volatile tcb_t *current_tcb;
u64 g_task_ticks64;
u32 g_task_ticks;

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

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);
extern void co_task_tick_init(int ticks);

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


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){
	co_task_tick_init(TICKS_PER_HZ);
	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);
	}
}

u32 co_task_current_stack(void) {
	return (u32)current_tcb->pstack;
}

u32 co_task_stack_size(void) {
	return current_tcb->stack_size;
}

void co_task_delay(int ms){
	current_tcb->start_tm_ms = ms + ticks_2_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();
		}
	}
}

u64 co_task_sys64_ticks(void) {
	return g_task_ticks64;
}

u64 co_task_sys64_ts(void) {
	return ticks_2_ms(g_task_ticks64);
}

/* 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 <= ticks_2_ms(g_task_ticks)){
			return next;
		}
	}
	if (current_tcb->start_tm_ms <= ticks_2_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();
	}
}

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

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