#include "ridhaOs.h"

/* Define a fixed linked list of Stack */
tControlBlock_t tcbs[NUM_OF_THREADS];

/* Define current Pointer to point to first of the list of Stack*/
tControlBlock_t *currentPt;

/* Each Thread will have STACKSIZE * 4. */
int32_t TCB_STACK[NUM_OF_THREADS][STACKSIZE];

void ridhaOsStackInit(int i)
{
	tcbs[i].stackPt = &TCB_STACK[i][STACKSIZE - 16];

	/* Set bit24 Thumb state bit to one, operate in thumb mode */
	TCB_STACK[i][STACKSIZE - 1] = (1U << 24); // PSR register

#ifdef DEBUG
	/* @Note: Block below need to delete after for debug purpose */
	/* Dummy stack content */
	TCB_STACK[i][STACKSIZE - 3] = 0xAAAAAAAA; // R14(LR)
	TCB_STACK[i][STACKSIZE - 4] = 0xAAAAAAAA; // R12
	TCB_STACK[i][STACKSIZE - 5] = 0xAAAAAAAA; // R3
	TCB_STACK[i][STACKSIZE - 6] = 0xAAAAAAAA; // R2
	TCB_STACK[i][STACKSIZE - 7] = 0xAAAAAAAA; // R1
	TCB_STACK[i][STACKSIZE - 8] = 0xAAAAAAAA; // R0

	TCB_STACK[i][STACKSIZE - 9] = 0xAAAAAAAA;  // R11
	TCB_STACK[i][STACKSIZE - 10] = 0xAAAAAAAA; // R10
	TCB_STACK[i][STACKSIZE - 11] = 0xAAAAAAAA; // R9
	TCB_STACK[i][STACKSIZE - 12] = 0xAAAAAAAA; // R8
	TCB_STACK[i][STACKSIZE - 13] = 0xAAAAAAAA; // R6
	TCB_STACK[i][STACKSIZE - 14] = 0xAAAAAAAA; // R7
	TCB_STACK[i][STACKSIZE - 15] = 0xAAAAAAAA; // R5
	TCB_STACK[i][STACKSIZE - 16] = 0xAAAAAAAA; // R4
#endif
}

#if (USE_STATIC_THREAD)
uint8_t ridhaOsAddThreads(StaticTask fn0, StaticTask fn1, StaticTask fn2)
{
	/* Disable Global interrupts */
	__disable_irq();
	tcbs[0].nextPt = &tcbs[1];
	tcbs[1].nextPt = &tcbs[2];
	tcbs[2].nextPt = &tcbs[0];

	/* Init Stacks for first thread */
	ridhaOsStackInit(0);

	/* Init Program counter (PC) to task functions */
	TCB_STACK[0][STACKSIZE - 2] = (int32_t)(fn0);

	/* Init Stacks for first thread */
	ridhaOsStackInit(1);

	/* Init Program counter (PC) to task functions */
	TCB_STACK[1][STACKSIZE - 2] = (int32_t)(fn1);

	/* Init Stacks for first thread */
	ridhaOsStackInit(2);

	/* Init Program counter (PC) to task functions */
	TCB_STACK[2][STACKSIZE - 2] = (int32_t)(fn2);

	/* Start from thread 0 */
	currentPt = &tcbs[0];

	/* Enable global IRQ */
	__enable_irq();

	return 1;
}
#endif

void ridhaOsStart(void)
{
	/* Init the Time base */
	ridhaOsSchedulerInit();

	/* Launch schedular */
	ridhaOsSchedulerLaunch();
}