/* resourcetest.c */
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "osek_kernel.h"
#include "task.h"

/* OSEK declarations */
DeclareCounter( SysTimerCnt );
DeclareTask(LowTask);
DeclareTask(MediumTask);
DeclareTask(HighTask);

/*Function Prototypes*/
void UpdateRowColumn();

U32 ctr = 0;
U8 row = 0;
U8 col = 0;

/* LEJOS OSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void)
{
	StatusType ercd;
	ctr++;

	ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */
	if(ercd != E_OK)
	{
		ShutdownOS(ercd);
	}
}

/* Definitions */

#define LOW	4000
#define MEDIUM	6000
#define HIGH	3000


TASK(LowTask)
{
	U32 i = 0;
	UpdateRowColumn();

	for (i = 0; i < LOW; i++)
	{
		systick_wait_ms((U32)1);
	  	display_goto_xy(col, row);
          	display_string("L ");
		display_unsigned((U32)ctr, 0);
		display_update();
	}	

	display_goto_xy(0, 7);
        display_string("CTR: ");
	display_unsigned((U32)ctr, 0);
	display_update();

	TerminateTask();
}

TASK(MediumTask)
{
	U32 i = 0;	
	UpdateRowColumn();

	for (i = 0; i < MEDIUM; i++)
	{
		systick_wait_ms((U32)1);
	  	display_goto_xy(col, row);
          	display_string("M ");
		display_unsigned((U32)ctr, 0);
		display_update();
	}	

	display_goto_xy(0, 7);
        display_string("CTR: ");
	display_unsigned((U32)ctr, 0);
	display_update();

	TerminateTask();
}

TASK(HighTask)
{
	U32 i = 0;	
	UpdateRowColumn();

	for (i = 0; i < HIGH; i++)
	{
		systick_wait_ms((U32)1);
	  	display_goto_xy(col, row);
          	display_string("H ");
		display_unsigned((U32)ctr, 0);
		display_update();
	}	

	display_goto_xy(0, 7);
        display_string("CTR: ");
	display_unsigned((U32)ctr, 0);
	display_update();

	TerminateTask();
}

void UpdateRowColumn()
{
	col = col + 8;
	if(col >=16)
	{
		row ++;
		col = 0;
	}
	if(row >=6)
	{
		 row = 0;
		 display_clear(0);
		 display_update();
	}
}
