#include "rtos.h"
#include "kernel.h"
#include "tisrw.h"

void SysTick_Handler()
{
	Kernel.Tick();
}

StackItem* OS_ContextSwitchHook(StackItem* sp)
{
	return Kernel.ContextSwitchHook(sp);
}

void TKernel::Sched()
{
    uint8_t nextPrty = getHighPriority(_readyProcessMap);
    if (nextPrty != _curProcPriority) {
        _schedProcPriority = nextPrty;
        *CPU_ICSR |= 0x10000000;   // Cause a context switch
        do {
        	__asm__ __volatile__ ("cpsie i");	// Enable interrupts
            __asm__ __volatile__ ("nop");
            __asm__ __volatile__ ("cpsid i");	// Disable interrupts
        }  while (!IsContextSwitchDone());
    }
}

void TKernel::Tick()
{
	TISRW ISR;

    _sysTickCount++;
    const uint32_t baseIndex = 1;
    for (uint32_t i = baseIndex; i < (PROCESS_COUNT + baseIndex); i++) {
    	if (_processTable[i]->WakeUpProcess() == true)
    		ReadyProcess(i);
    }
    Scheduler();
}

void TKernel::SchedISR()
{
    uint8_t nextPrty = getHighPriority(_readyProcessMap);
    if (nextPrty != _curProcPriority) {
        _schedProcPriority = nextPrty;
        *CPU_ICSR |= 0x10000000;   // Cause a context switch
    }
}

StackItem* TKernel::ContextSwitchHook(StackItem* sp)
{
    _processTable[_curProcPriority]->SetSP(sp);
    sp = _processTable[_schedProcPriority]->GetSP();
    _curProcPriority = _schedProcPriority;
    return sp;
}

void TKernel::Run()
{
    StackItem* sp = _processTable[pr0]->GetSP();
    OS_Start(sp);
}