#include "scb.h"
#include "rcc.h"
#include "kernel.h"
#include "tisrw.h"
#include "ticker.h"

ProcessMap			Kernel::_readyProcessMap	= ( (1 << (ThreadCount + 1)) - 1);  // set all processes ready
BaseThread* 		Kernel::_processTable[ThreadCount + 1];

volatile uint32_t	Kernel::_curProcPriority 	= pr0;
volatile uint32_t 	Kernel::_schedProcPriority;
volatile uint32_t 	Kernel::_ISR_NestCount		= 0;
volatile uint32_t	Kernel::_sysTickCount   	= 0;
StackItem* Kernel::_sp = 0;

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

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

void Kernel::Sched(void)
{
    uint32_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 Kernel::Scheduler(void)
{
	if (_ISR_NestCount) {
		return;
	}	else {
		Sched();
	}
}

void Kernel::Tick()
{
	TISRW ISR;

    _sysTickCount++;
    const uint32_t baseIndex = 1;
    for (uint32_t i = baseIndex; i < (ThreadCount + baseIndex); i++) {
    	if (_processTable[i]->WakeUpThread() == true)
    		ReadyThread(i);
    }
    Scheduler();
}

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

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

uint32_t Kernel::getHighPriority(ProcessMap pm)
{
	uint32_t clzero;
	asm ("clz\t%0, %1": "=r" (clzero): "r" (pm));
	return (31 - clzero);
}

ProcessMap Kernel::getPrioTag(const uint32_t pr)
{
	return static_cast<ProcessMap> (1 << pr);
}

void Kernel::setPrioTag(ProcessMap& pm, const ProcessMap prioTag)
{
	pm |=  prioTag;
}

void Kernel::clrPrioTag(ProcessMap& pm, const ProcessMap prioTag)
{
	pm &= ~prioTag;
}

void Kernel::ReadyThread(const uint32_t pr)
{
	ProcessMap prioTag = getPrioTag(pr);
	setPrioTag( _readyProcessMap, prioTag);
}

void Kernel::BlockThread(const uint32_t pr) {
	ProcessMap prioTag = getPrioTag(pr);
	clrPrioTag( _readyProcessMap, prioTag);
}

void Kernel::Run(uint32_t timerFrequencyInHz)
{
	enum {
		PendSV_Priority 		= 0xFF,
		SysTick_Priority		= 0xFE
	};

	uint32_t	SYS_Clock;
	uint32_t	AHB_Clock;
	uint32_t	APB1_Clock;
	uint32_t	APB2_Clock;
	Ticker		theSystickTimer;

	SCB& scb(*new SCB);
	RCC& rcc(*new RCC);
    _sp = _processTable[pr0]->GetSP();

	scb.SetSystemHandlerPriority(SCB::PendSV_IRQn,  PendSV_Priority);
	rcc.GetClocks(SYS_Clock, AHB_Clock, APB1_Clock, APB2_Clock);
	uint32_t sysTickFrequencyInHz = (AHB_Clock >> 3);
	uint32_t sysTickDivider = (sysTickFrequencyInHz / timerFrequencyInHz) - 1;
	scb.SetSystemHandlerPriority (SCB::SysTick_IRQn, SysTick_Priority);
	theSystickTimer.Configure(sysTickDivider);
    __asm__ __volatile__ (
    	"LDR	 R2,  %0\t\n"
    	"MOVS    R0,  #0\n\t"
    	"MSR     PSP, R0\n\t" :: "m" (_sp)
    );
	scb.PendSVC();
	__asm__ __volatile__ ("cpsie i");
}

volatile bool Kernel::IsContextSwitchDone()
{
	return (_curProcPriority == _schedProcPriority);
}

BaseThread* Kernel::Running(void)
{
	return _processTable[_curProcPriority];
}

void Kernel::RegisterThread(BaseThread* const p, uint32_t priority)
{
	_processTable[priority] = p;
}


void Kernel::PendSVC_Handler(void)
{
#if 0
    __asm__ __volatile__(
    		"CPSID   I\n\t"							// Prevent interruption during context switch
    		"MRS     R0, PSP\n\t"					// PSP is process stack pointer
    		"CBZ     R0, nosave\n\t"				// Skip register save the first time
    		"LDR     R0, %0"
    	    "STMDB   %0!, {R4-R11}\n\t" : "=m" (_sp)	// Save remaining regs r4-11 on process stack
    );
#endif
    _processTable[_curProcPriority]->SetSP(_sp);
    _curProcPriority = _schedProcPriority;
    _sp = _processTable[_schedProcPriority]->GetSP();
    __asm__ __volatile__ (
    		"nosave:\n\t"
        	   "LDR	 R0, %0\t\n"
    		   "MSR psp, R0\n\t" :: "m" (_sp)
    );
    __asm__ __volatile__(
    		"LDMIA 	 R0!, {R4-R11}\n\t"      			// Restore r4-11 from new process stack
    		"MSR     PSP, %0\n\t"           			// Load PSP with new process SP
    		"ORR     LR, LR, #0x04\n\t"				// Ensure exception return uses process stack
    		"CPSIE   I\n\t"
    		"BX      LR\n\t"						// Exception return will restore remaining context
    );
}