#include "helloworld.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

extern void ecrobot_checkExitButton(void);

void jsp_systick_low_priority(void) {
	isig_tim();
	ecrobot_checkExitButton();
}

const U8 target_subsystem_name[] = "JSP HelloWorld";

void ecrobot_device_initialize(void) {
	ecrobot_init_bt_connection();
	ecrobot_init_sonar_sensor(NXT_PORT_S2);
}

void ecrobot_device_terminate(void) {
	ecrobot_set_motor_speed(NXT_PORT_B, 0);
	ecrobot_set_motor_speed(NXT_PORT_C, 0);
	ecrobot_term_bt_connection();
	ecrobot_term_sonar_sensor(NXT_PORT_S2);
}

void cyc0(VP_INT exinf) {
	iact_tsk(TSK0);
}

void cyc1(VP_INT exinf) {
	iact_tsk(TSK1);
}

void tsk0(VP_INT exinf) {
	ecrobot_status_monitor();
	ext_tsk();
}

void tsk1(VP_INT exinf) {
	S32 s = ecrobot_get_sonar_sensor(NXT_PORT_S2);
	if (s < 10 || s == 255) {
		ecrobot_set_motor_speed(NXT_PORT_B, 0);
		ecrobot_set_motor_speed(NXT_PORT_C, 0);
	} else if (s < 25) {
		ecrobot_set_motor_speed(NXT_PORT_B, 25);
		ecrobot_set_motor_speed(NXT_PORT_C, 25);
	} else {
		ecrobot_set_motor_speed(NXT_PORT_B, 100);
		ecrobot_set_motor_speed(NXT_PORT_C, 100);
	}
	ext_tsk();
}
