#include "timebase.h"
#include "fsl_device_registers.h"

#define CTRL_ENABLE			(1U<<0)
#define CTRL_TICKINT		(1U<<1)
#define CTRL_CLKSRC			(1U<<2)
#define CTRL_COUNTFLAG	(1U<<16)
#define MAX_DELAY				0xFFFFFFFFU

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;

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

void delay_s(uint32_t delay)
{
	delay_ms(delay * 1000);
}

void delay_ms(uint32_t delay)
{
	uint32_t tickstart = get_tick();
	uint32_t wait = delay;
	if(wait < MAX_DELAY)
	{
		wait += (uint32_t)(tick_freq);
	}
	
	while((get_tick() - tickstart) < wait){}
	
}

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

void timebase_init(void)
{
	/* Reload the timer with number of cycles per second */
	/* Getting SystemCoreClock divide by 1000 to get interrupt every 1ms */
	SysTick->LOAD = (SystemCoreClock / 1000) - 1;
	
	/* Clear Systick current value register */
	SysTick->VAL = 0x00;
	
	/* Select Internal Clock source */
	SysTick->CTRL = CTRL_CLKSRC;
	
	/* Enable interrupt */
	SysTick->CTRL |= CTRL_TICKINT;

	/* Enable Systick */
	SysTick->CTRL |= CTRL_ENABLE;

	/* Enable Global Interrupt */
	__enable_irq();

}

void SysTick_Handler(void){
	tick_increment();
}