#include "led.h"
#include "uart.h"
#include "fsl_debug_console.h"
#include "fsl_power.h"
#include "fsl_common.h"
#include "ridhaOs.h"

int32_t semaphore1, semaphore2;


typedef uint32_t TaskProfiler;
TaskProfiler Task0_Profiler, Task1_Profiler, Task2_Profiler;

void task0(void);
void task1(void);
void task2(void);
void SystemInitHook()
{
	SystemCoreClockUpdate();
	__set_MSPLIM(0x20000000);
	
}

int main(void){

	SystemInitHook();
	uart_init();
	
	/* Init Semaphore */
	ridhaOsSemaphoreInit(&semaphore1, 1);
	ridhaOsSemaphoreInit(&semaphore2, 0);
	
	/* Add threads */
	ridhaOsAddThreads(&task0, &task1, &task2);
	ridhaOsStart();
	
	/* We can't reach this point of program */
	while(1);
}


void task0(void)
{
	Task0_Profiler = 0;
	while(1)
	{
		ridhaOsSemaphoreWait(&semaphore1);
		PRINTF("Task 0\n");
		ridhaOsSemaphoreSet(&semaphore2);

		Task0_Profiler++;
	}
}

void task1(void)
{
	Task1_Profiler = 0;
	while(1)
	{
		ridhaOsSemaphoreWait(&semaphore2);
		PRINTF("Task 1\n");
		ridhaOsSemaphoreSet(&semaphore1);

		Task1_Profiler++;
	}
}

void task2(void)
{
	Task2_Profiler = 0;
	while(1)
	{
		//PRINTF("Task 2\n");
		Task2_Profiler++;
	}
}