void init_epuck()
{
	e_start_agendas_processing();
	e_init_port();
	e_init_motors();
	e_init_uart1();
	e_init_uart2();
	e_init_ad_scan();
//	init_IR();
}

void slight_left()
{
	long j=0;
	for (j=0; j<1000;j++)
	{
		e_set_speed_right(900);
		e_set_speed_left(-200);
	}
}

void slight_right()
{
	long j=0;
	for (j=0; j<1000;j++)
	{
		e_set_speed_right(-200);
		e_set_speed_left(900);
	}
}


void move(long x, int speed)
{
	long i=0;
	if (x==0) //no time defined keeps moving constantly
	{
		e_set_speed_right(speed);
		e_set_speed_left(speed);
	}
	else
	{
		for (i=0; i<x; i++)
		{
			e_set_speed_right(speed);
			e_set_speed_left(speed);
		}
	}
}

void tracking()
{
	int a,b,c,d,e,f,g,h = 0;
	int left_light;
	int right_light;
	a=e_get_ambient_light(0);
	b=e_get_ambient_light(1);
	c=e_get_ambient_light(2);
	d=e_get_ambient_light(3);
	e=e_get_ambient_light(4);
	f=e_get_ambient_light(5);
	g=e_get_ambient_light(6);
	h=e_get_ambient_light(7);
	left_light = (e+f+g+h)/4; //average value of light to the left
	right_light = (a+b+c+d)/4; //average value of light to the right
	btcomSendInt(left_light);
	btcomSendChar('-');
	btcomSendInt(right_light);
	btcomSendChar(' ');
	if(right_light<left_light) //brighter source on the right
		slight_right();
	else if (left_light<right_light) //brighter source on the left
		slight_left();
	else 
		move(0, 200);
}


int get_selector()
{
  return SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
}

int main (void)
{
	init_epuck();
	int selector;
	int value;
    long int j;

	selector=get_selector();
	while(1)
	{
	if(selector == 0)
		{
		e_set_led(0,1);
		e_set_led(1,1);
		e_set_led(2,1);
		e_set_led(3,1);
		e_set_led(4,1);
		e_set_led(5,1);
		e_set_led(6,1);
		e_set_led(7,1);
		}

	else if(selector == 1)
	{
		value = (e_get_prox(0)+e_get_prox(7))/2; //proximity of obstacle in front
	  		if(value > 200)		  			
				{
				e_blink_led();
				move(0,0);
				}
			else
				{
				tracking();
				e_led_clear();
				}
			for(j=0; j<100000; j++) { asm("nop"); }
	}


	else if(selector == 2)
	{
		value = e_get_ambient_light(0);
		btcomSendInt(value);
		btcomSendChar(' ');
		for(j=0; j<100000; j++) { asm("nop"); }
	}
	}
}