#include "ridhaOsScheduler.h"

/* Define current Pointer to point to first of the list of Stack*/
extern tControlBlock_t *currentPt;
volatile uint32_t g_curr_tick;
volatile uint32_t g_curr_tick_p;
volatile uint32_t tick_freq = 1;

uint8_t tick_freq_div = 1;

static void tick_increment(void)
{
  g_curr_tick += tick_freq;
}

static inline void ridhaOsSchedulerReset(void)
{
  /* Reset Systick configuration*/
  SysTick->CTRL = 0x0000;

  /* Clear Systick current Value */
  SysTick->VAL = 0;
}

void ridhaOsSchedulerDelayS(uint32_t delay)
{
  ridhaOsSchedulerDelayMS(delay * 1000);
}

void ridhaOsSchedulerDelayMS(uint32_t delay)
{
  uint32_t tickstart = ridhaOsSchedulerGetTick();
  uint32_t wait = delay;
  if (wait < MAX_DELAY)
  {
    wait += (uint32_t)(tick_freq);
  }

  while ((ridhaOsSchedulerGetTick() - tickstart) < wait)
  {
  }
}

uint32_t ridhaOsSchedulerGetTick(void)
{
  __disable_irq();
  g_curr_tick_p = g_curr_tick;
  __enable_irq();
  return g_curr_tick_p;
}

void ridhaOsSchedulerInit(void)
{
  /* Reset registers */
  ridhaOsSchedulerReset();

  /* Reload the timer with number of MS */
  SysTick->LOAD = ((SystemCoreClock / TICK_RATE_HZ) * ROUND_ROBIN_QUANTA) - 1;

  /* Clear Systick current value register */
  SysTick->VAL = 0x00;

  /* Select Internal Clock source */
  SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk;

  /* Set Systick to low priority */
  NVIC_SetPriority(SysTick_IRQn, 15);

  /* Enable SysTick interrupt */
  SysTick->CTRL |= SysTick_CTRL_TICKINT_Msk;
}

void ridhaOsSchedulerLaunch(void)
{
  /* Load Address of currentPt into R0 */
  __asm("LDR  R0, =currentPt");

  /* Load R2 from address R0, i.e R2 =currentPt */
  __asm("LDR  R2, [R0]");

  /* Load Cortex M SP from address equals R2, i.e. SP = currentPt->stackPt */
  __asm("LDR  SP, [R2]");

  /* Restore r4, r5, r6, r7, r8, r9, r10, r11 */
  __asm("POP  {R4-R11}");

  /* Restore R12 */
  __asm("POP  {R12}");

  /* Restore r0, r1, r2, r3 */
  __asm("POP {R0-R3}");

  /* Skip LR */
  __asm("ADD SP, SP, #4");

  /* Create a new start location by poping LR */
  __asm("POP {LR}");

  /* Skip xPSR by adding 4 to SP */
  __asm("ADD SP,SP, #4");

  /* Enable global interrupt */
  __asm("CPSIE	    I");

  /* Return from exception and restore r0 to r3, lr, pc, psr */
  __asm("BX			LR");
}

__attribute__((naked)) void SysTick_Handler(void)
{

  /* SUSPEND CURRENT THREAD */
  /* DISABLE GLOBAL INTERRUPTS */
  __asm("CPSID		I");
  /* Save Other Register not pushed by interrupt (R4->R11) */
  __asm("PUSH			{R4-R11}");
  /* Load Address of currentPT int R0 */
  __asm("LDR R0, =currentPt");
  /* Load r1 from address equals r0, i.e r1 =currentPt */
  __asm("LDR R1, [R0]");
  /* Store Cortex-M SP at address equales to R1, i.e save SP into tcb */
  __asm("STR SP, [R1]");

  /* CHOOSE NEXT THREAD */
  /* Load r1 from a location 4 bytes above address r1, i.e next r1 =currentPt->next*/
  __asm("LDR R1, [R1,#4]");
  /* Store r1 @Address equals r0, i.e currentPt = r1 */
  __asm("STR R1, [R0]");
  /* Load Cortext M SP from @ equals r1, i.e SP = current->stackPt */
  __asm("LDR SP, [R1]");
  /* Restore R4-R11 registers */
  __asm("POP {R4-R11}");
  /* Enable global interrupt */
  __asm("CPSIE			I");
  /* Return from exception and restore r0 to r3, lr, pc, psr */
  __asm("BX			LR");
}


void ridhaOsSchedulerThreadYield(void)
{
	/* Clear Systick current value register */
	SysTick->VAL = 0;
	
	/* Trigger Systick */
	SCB->ICSR |= SCB_ICSR_PENDSTSET_Msk;
		
}