#include <task.h>
#include <heap.h>
#include <stm32l476xx.h>

typedef struct {
	uint32_t *sp;
	uint8_t use;
	uint32_t *stack;
	void (*code)(void);
} task_t;

#define MAX_TASKS	4

static task_t tasks[MAX_TASKS];
static int next_idx = 0;

void task_init(void (*init)(void))
{
	for (int i = 0; i < MAX_TASKS; i++)
		tasks[i].use = 0;

	task_start(init, 1024);
	asm("\
		msr psp, %0; \
		mrs r0, control; \
		orr r0, r0, #2; \
		msr control, r0; \
		isb; \
	" :: "r" (tasks[0].sp));

	init();

	// force switch to tasking, call pendsv
	//SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
}

extern void _exit(int);
void task_exit(void)
{
	// TODO free stack?
	asm("cpsid i"); // hope to catch next_idx
	tasks[next_idx].use = 0;
	asm("cpsie i");
	while (1); // bye
	//_exit(0);
}

void task_start(void (*task)(void), uint16_t stackSize)
{
	asm("cpsid i"); // just to be safe

	for (int i = 0; i < MAX_TASKS; i++) {
		if (tasks[i].use == 0) {
			tasks[i].stack = hmalloc(stackSize);
			tasks[i].sp = tasks[i].stack + stackSize - 16;
			tasks[i].sp[13] = (uint32_t)task_exit;
			tasks[i].sp[14] = (uint32_t)task;
			tasks[i].sp[15] = 0x01000000;
			tasks[i].use = 1;
			tasks[i].code = task;
			asm("cpsie i");
			return;
		}
	}

	// TODO handle error
}

__attribute__ ((naked))
void PendSV_Handler(void) 
{
	// save state
	//stmdb r0!, {r4-r11};
	asm("\
		cpsid i; \
		isb; \
		dsb; \
		mrs r0, psp; \
		stmdb r0!, {r4-r11}; \
		mov %0, r0; \
	" : "=r" (tasks[next_idx].sp));

	// find next task (round-robin style)
	do {
		if (++next_idx == MAX_TASKS) {
			next_idx = 0;
			break; // task 0 better exist
		}
	} while (tasks[next_idx].use == 0);

	// restore
	//ldmia r0!, {r4-r11};
	asm("\
		mov r0, %0; \
		ldmia r0!, {r4-r11}; \
		msr psp, r0; \
		isb; \
		dsb; \
	" :: "r" (tasks[next_idx].sp));

	// end
	asm("\
		mov r0, #0xFFFFFFFD; \
		cpsie i; \
		bx r0; \
	");
}