#include "kernel.h"
#include "ecrobot_interface.h"

#define PORTA NXT_PORT_A
#define PORTB NXT_PORT_B
#define PORTC NXT_PORT_C
#define PORT1 NXT_PORT_S1
#define PORT2 NXT_PORT_S2
#define PORT3 NXT_PORT_S3
#define PORT4 NXT_PORT_S4

#define SOUND 600
#define SPEED 50

void move_forward(void);

const char target_subsystem_name[] = "sample2";

//OSEK declarations
DeclareCounter(SysTimerCnt);

//LEJOS OSEK hooks
void ecrobot_device_initialize()
{
	nxt_motor_set_speed(PORTA, 0, 1);
	nxt_motor_set_speed(PORTC, 0, 1);
}

void ecrobot_device_terminate()
{
	nxt_motor_set_speed(PORTA, 0, 1);
	nxt_motor_set_speed(PORTC, 0, 1);
}

//LEJOS OSEK hook to be invoked from an ISR in category 2
void user_1ms_isr_type2(void)
{
	StatusType ercd;
	ercd = SignalCounter(SysTimerCnt);
	if(ercd != E_OK)
	{
		ShutdownOS(ercd);
	}
}

//DisplayTask
TASK(DisplayTask)
{
	ecrobot_status_monitor(target_subsystem_name);

	TerminateTask();
}

//ActionTask
TASK(ActionTask)
{
	while(true){
		if ( ecrobot_get_sound_sensor(PORT1) < SOUND )
		{
			move_forward();
		}
	}

	TerminateTask();
}

void move_forward(void)
{
	nxt_motor_set_speed(PORTA, SPEED, 1);
	nxt_motor_set_speed(PORTC, SPEED, 1);
	systick_wait_ms(3000);
	nxt_motor_set_speed(PORTA, 0, 1);
	nxt_motor_set_speed(PORTC, 0, 1);
}