#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 WALL_DIS 30	//(cm)
#define SPEED 50
#define BACK_SPEED -50

void move_forward(void);
void move_back(void);

const char target_subsystem_name[] = "sample3";

//OSEK declarations
DeclareCounter(SysTimerCnt);

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

void ecrobot_device_terminate()
{
	ecrobot_term_sonar_sensor(PORT2);
	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((ecrobot_get_sonar_sensor(PORT2) <= 0) || (ecrobot_get_sonar_sensor(PORT2) == 255)){
		systick_wait_ms(500);
	}

	while(true){
		if (ecrobot_get_sonar_sensor(PORT2) > WALL_DIS)
		{
			move_forward();
		}else{
			move_back();
		}
		systick_wait_ms(50);
	}

	TerminateTask();
}

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

void move_back(void)
{
	nxt_motor_set_speed(PORTA, BACK_SPEED, 1);
	nxt_motor_set_speed(PORTC, BACK_SPEED, 1);
	systick_wait_ms(3000);
}