#define NEAR 18
#define PROXIMITE 30
#define SPEED 250
#define DEGPSEC 62 // 72
#ifndef CHANNEL
#define CHANNEL   1    // slave channel can be 1, 2 or 3
#endif

#ifndef MAILBOX
#define MAILBOX   0    // 0 to 9
#endif
#define CONNECTION  1
#define SERV_NAME   "eurecom-os-server"             // The name of the server

#define MSG_ACTION  0
#define MSG_ACK     1
#define MSG_LEAD    2
#define MSG_START   3
#define MSG_STOP    4
#define MSG_WAIT    5
#define MSG_CUSTOM  6
#define MSG_KICK	7
#define MSG_CANCEL  8

// MUTEX //
mutex motorMutex;
mutex pinceMutex;

// MODES ET INFOS //
bool attrapee = false; 
bool leader =true;
bool positionDonnee=false;
bool obstacle=false;
bool balleDevant=false;
bool ballePeutEtreTrouvee=false;
int minDistanceBalle= 30;

// POSITIONS INITIALES //
int angleDepart;
int x=100;
int y=20;
int angleRobot=0;
int directionInitiale;
int angleASuivre;

// POSITIONS CIBLES //
int targetX=120;
int targetY=100;

// COMMUNICATION / SNAKE //
int idPrevious;
int idNext;
int ourID=12;
int distanceReceived;
int oldOrderDistance;
int distanceAvantCancel;
int distanceCanceled;
int angleReceived = 0;
int angleAnnonce=0;
int oldAngleReceived ;
int speedReceived;
int delayReceived;
int rank;
int snakeSize;
int angleAEnvoyer;
int distanceAEnvoyer;
int speedAEnvoyer;

// ADD VARIABLES //
int idMessage=0;
bool waitInfo=false;
bool droitAvancer=true;
bool cancelMessage = false;
bool startRobot=false;
int lastAction =-1;
int rankKickOut;
bool actionExecution = false;
int id_ack;
int state;
byte message[10];

// // ******************************************** COMMUNICATION BLUETOOTH ******************************************** //

///////////////////////////////// CAST FONCTION ///////////////////////////////////
inline int byteToInt(byte byteVal){
	short c = byteVal;
	int x = c;
	return x;
}
inline byte intToByte(int intVal)
{
	short c = intVal;

	return (c & 0x00FF);	
}


///////////////////////////////// GET MESSAGE TASK : RECEIVE MESSAGES, MAKE ACTIONS , && SEND MESSAGES ///////////////////////////////////
char getMessage (byte &msg[]) {
    until (BluetoothStatus(CONNECTION) == NO_ERR);  // Wait for connection
    string __msg;
    char r = ReceiveMessage(0,1,__msg);
    if (r != 0)
        return 0;

    ArrayInit(msg,0,59);
    StrToByteArray (__msg, msg);
    r = ArrayLen(msg);
    if (r < 5)
        return 0;
	
    return r;
}
inline void sendMessage (byte msg[]) {
    CommBTWriteType args2;
    args2.Connection = CONNECTION;
    args2.Buffer= ByteArrayToStr(msg);

    SysCommBTWrite(args2);
    //TextOut(0, LCD_LINE2,       "Trying to write");
    int try=4;
    while (BluetoothStatus (CONNECTION) == STAT_COMM_PENDING && try<4){
        Wait (10);
        try+=1;
    }

    if (BluetoothStatus (CONNECTION) == NO_ERR)
        TextOut(0, LCD_LINE2,"Write Done");
    //else
        //TextOut(0, LCD_LINE2,   "Write Failed. Try again");
    	// Wait(100);
     // 	ClearScreen();
    	// CommBTWriteType args1;
    	// args1.Connection = CONNECTION;
    	// args1.Buffer= ByteArrayToStr(msg);

    	// SysCommBTWrite(args1);
    	// //TextOut(0, LCD_LINE2,       "Trying to write");
    	// while (BluetoothStatus (CONNECTION) == STAT_COMM_PENDING)
     //    	Wait (100);
}

///////////////////////////////// FONCTIONS RESPOND TO OTHER ROBOTS OR SEVER ///////////////////////////////////

void respondAction(byte msg[]) // Id = 0
{
	
	lastAction = 0;
	// Recuperer les valeurs
	short tmp1;
	short tmp0;
	tmp1 = msg[1];
	tmp0 = msg[0];
	short tmp5;
	short tmp6;
	tmp5 = msg[5];
	tmp6 = msg[6];
	short tmp9;
	short tmp8;
	tmp9 = msg[9];
	tmp8 = msg[8];

	idMessage = tmp1*256 + tmp0; // & 0xFF can be added if sign issues like : id = ((msg[0] & 0xFF) << 8  ) | msg[1]; 
	id_ack = idMessage ;
	oldAngleReceived=angleReceived;
	angleReceived = tmp6*256 + tmp5;
	distanceReceived = byteToInt(msg[7]);
	speedReceived = tmp9*256 + tmp8;
}
bool respondAck(byte msg[]) // Id = 1
{	// Recuperer les valeurs
	idMessage = (msg[1]*256 + msg[0]);
	id_ack = msg[6] + msg[5]*256;
	state = byteToInt(msg[7]);
	
	if(state == 0)
	{
		// REUSSI
		return true;
	}
	else
	{
		// ERROR;
		return false;
	}
}
void respondLead(byte msg[]) // Id = 2
{
	lastAction = 2;	
	// Recuperer les valeurs
	short tmp1;
	short tmp0;
	tmp1 = msg[1];
	tmp0 = msg[0];
	idMessage = (tmp1*256 + tmp0);
	TextOut(0,LCD_LINE1,"LEAD");
	Wait(1000);
	ClearScreen();
}
bool respondStart(byte msg[]) // Id = 3
{
  	TextOut(0,LCD_LINE1,"BODY START");
	lastAction = 3;	
	// Get Value && put it in 
	short tmp1;
	short tmp0;
	tmp1 = msg[1];
	tmp0 = msg[0];
	idMessage = (tmp1*256 + tmp0);
	rank = byteToInt(msg[5]);
	snakeSize = byteToInt(msg[6]);
	idPrevious = byteToInt(msg[7]);
	idNext = byteToInt(msg[8]);

	if((rank == 0 ) && (msg[7]!=0xFF))
	{
		leader = true;
		TextOut(0,LCD_LINE1,"LEADER");

	}
	else
	{
//		leader = false;
		TextOut(0,LCD_LINE1,"SLAVE");
	}

	if(msg[8] == 0xFF && (rank+1)!=snakeSize)
	{
		TextOut(0,LCD_LINE1,"ERROR: CANT BE LAST && NOT");
		return false;
	}

	return true;

}
void respondStop(byte msg[]) // Id = 4
{
	lastAction = 4;
	short tmp1;
	short tmp0;
	tmp1 = msg[1];
	tmp0 = msg[0];
	idMessage = (tmp1 + tmp0*256) ;
}
void respondWait(byte msg[]) // Id = 5
{

	lastAction = 5;
	short tmp1;
	short tmp0;
	tmp1 = msg[1];
	tmp0 = msg[0];
	idMessage = (tmp1 + tmp0*256);
	delayReceived = byteToInt(msg[5]);

}
void respondCustom(byte msg[]) // Id = 6
{

}
void respondKick(byte msg[]) // Id = 7
{
	lastAction = 7;
	short tmp1;
	short tmp0;
	tmp1 = msg[1];
	tmp0 = msg[0];
	idMessage = (tmp1*256 + tmp0);
	rankKickOut = byteToInt(msg[5]);
	idPrevious =  byteToInt(msg[6]);
	idNext =  byteToInt(msg[7]);
	TextOut(0,LCD_LINE1,"KICK OUT");
}
void respondCancel(byte msg[]) // Id = 8
{
	lastAction = 8;
	short tmp1;
	short tmp0;
	tmp1 = msg[1];
	tmp0 = msg[0];
	idMessage = (tmp1 + tmp0*256);
	distanceCanceled = byteToInt(msg[5]);
}
///////////////////////////////// SEND MESSAGE ///////////////////////////////////

void sendAction(int id,char src,char dst,int angle,char dist,int speed)
{
	if(src != dst)
	{	
		byte message[10];

		ClearScreen();
		NumOut(0,LCD_LINE2,angle);
		NumOut(0,LCD_LINE3,speed);
		TextOut(0,LCD_LINE4,"sendAction");
		Wait(1500);
		ClearScreen();

		message[1] = id & 0xF0;
		message[0] = id & 0x0F;
		message[2] = src;
		message[3] = dst;
		message[4] = 0;
		if (angle>255){
			message[5]=angle & 0x0FF;
			message[6]= 0x01;
		}
		else{
			message[5] = angle ;
			//message[6] = 0x00;
		}		
		if (speed>255){
			message[8]=255;
			message[9]=(speed) & 0xF00;
		}
		else{
			message[8] = speed;
			//message[9] = 0x00;
		}
		message[7] = dist;
		sendMessage(message);
	}
}


void sendAck(int id,char src,char dst,int id_ack,char state)
{
	if(src != dst)
	{	
		byte message[8];
		message[1] = (id & 0xF0);
		message[0] = (id & 0x0F);
		message[2] = src;
		message[3] = dst;
		message[4] = 1;
		message[6] = id_ack & 0xF0;
		message[5] = id_ack & 0x0F;
		// TextOut(0,LCD_LINE2,"SEND ACK");
		// NumOut(0,LCD_LINE3,id_ack);
		// Wait(1000);

		message[7] = state;
		sendMessage(message);
	}
}

void sendLead(int id,char src,char dst)
{
	if(src != dst)
	{	
		byte message[5];
		message[1] = id & 0xF0;
		message[0] = id & 0x0F;
		message[2] = src;
		message[3] = dst;
		message[4] = 2;

		// PERTE DU LEADERSHIP
		leader = false;
		sendMessage(message);
	}
}

void sendCancel(int id,char src,char dst,char dist)
{
		byte message[6];
		message[1] = id & 0xF0;
		message[0] = id & 0x0F;
		message[2] = src;
		message[3] = dst;
		message[4] = 8;
		message[5] = dist;
		sendMessage(message);
	
}
// // ******************************************** EASY SEND FOR CHABERT ******************************************** //

void easySendLead(){
	if(idNext != 255)
	{
		sendLead(idMessage++,intToByte(ourID),intToByte(idNext));
	}
}

void easySendCancel(int distanceDejaParcourue){
	if(intToByte(idNext) != 0xFF)// On n'est pas dernier
	{	
		sendCancel(idMessage++,intToByte(ourID),intToByte(idNext),intToByte(distanceDejaParcourue));
	}

}

void easySendWait(int secondToWait){

	if(intToByte(idNext) != 0xFF)
	{
		byte message[6];
		message[1] = (idMessage & 0x0F);
		message[0] = (idMessage & 0xF0);
		message[2] = (ourID);
		message[3] = (idNext);
		message[4] = 5;
		message[5] = intToByte(secondToWait);
		sendMessage(message);
	}
}

void easySendAction(int dst, int speed, int angle){
	if(idNext != 255)// On est pas dernier
	{	
		NumOut(0,LCD_LINE6,angle);	
		sendAction(idMessage++,intToByte(ourID),intToByte(idNext),(angle),intToByte(dst),(speed));
	}
}

// // ******************************************** DONNEES ******************************************** //

										// *********** OBTENIR ANGLE ROBOT *********** //
inline int getAngle(){
	Wait(750);
	int angle = SensorHTCompass(IN_3);
	return angle;
}

// ******************************************** PINCE ******************************************** //

void release(){
	Acquire(pinceMutex);
	OnFwd(OUT_C, 100);
	Wait(500);
	Float(OUT_C);
	Release(pinceMutex);
}

void close(){
	Acquire(pinceMutex);
	OnRev(OUT_C, 80);
	Wait(600);
	Float(OUT_C);
	Release(pinceMutex);
}
									// *********** ATTRAPER SUITE AUX REACTIONS DES CAPTEURS *********** //
task catch(){	
	until(attrapee){
		if (SENSOR_1 == 5 || SENSOR_1 == 2){
			int essais=0;
			close();
			while (SENSOR_4!=1 && essais<5){
				release();
				close();
				essais+=1;
			}
			if (SENSOR_4 == 1) {
				TextOut(0,LCD_LINE2,"Balle Attrapée");
				attrapee=true;	
			}
			else{
				release();
				attrapee=false;	
			}
		}
	}
}

// // ******************************************** MOUVEMENTS ******************************************** //
	
										// *********** AVANCER *********** //

void avancerTime(int time){
	Acquire(motorMutex);
	OnFwdReg(OUT_AB,75,OUT_REGMODE_SYNC);
	Wait(time);
	Release(motorMutex);
	Float(OUT_AB);
}

int avancerTimeRapport(int time,int rapport, int distanceTotale){
	int interval=time/10;
	int distInterval=distanceTotale/10;
	int i = 0;
	Acquire(motorMutex);
	OnFwdReg(OUT_AB,rapport,OUT_REGMODE_SYNC);
	while (i<10 && droitAvancer){
		if (SensorUS(IN_2)>NEAR){
			Wait(interval);	
		}
		else{
			Float(OUT_AB);
		}
		i+=1;
		NumOut(0,LCD_LINE1,i);
	}
	Float(OUT_AB);
	Release(motorMutex);
	return ((i+1)*distInterval);
}

void avancerDist(int distance){
	int time;
	int angleAEnvoyer;


	angleAEnvoyer=getAngle()-directionInitiale;
	if (angleAEnvoyer<0) angleAEnvoyer+=360;

	if (leader)
	{
		easySendAction(distance,SPEED,angleAEnvoyer); // TODOBLUETOOTH SEND DIST SPEED/10
		angleAnnonce=angleAEnvoyer;
	}
	time=(distance*10*1000/SPEED);
	avancerTime(time);
	distanceAEnvoyer=distance;
	speedAEnvoyer=SPEED;
}

void approcheFurtive(int distance){
	int time;
	time=(distance*10*1000/(SPEED/1.5));
	Acquire(motorMutex);
	OnFwdReg(OUT_AB,35,OUT_REGMODE_SYNC);
	Wait(time);
	Float(OUT_AB);
	Release(motorMutex);
}

int avancerDistSpeed(int distance, int speed){ 		// TODOBLUETOOTH : Fonction a appeler pour suivre commandes
	int timeAAvancer = distance*1000*10/speed ;
	int distanceParcourue;
	until (waitInfo==false){
		Wait(500);
	}
	if (speed<100 && speed>0){
		distanceParcourue = avancerTimeRapport(timeAAvancer,35*speed/100,distance); // Rapport 3.5
	}
	else if(speed<=150 && speed>=100){
		distanceParcourue = avancerTimeRapport(timeAAvancer,32*speed/100,distance);
	}
	else if(speed<=200 && speed>100){
		distanceParcourue = avancerTimeRapport(timeAAvancer,3*speed/10,distance);
	}
	else{
		distanceParcourue = avancerTimeRapport(timeAAvancer,28*speed/100,distance); // Rapport 2.8
	}

	return distanceParcourue;
}

void reculerTime(int time){
	Acquire(motorMutex);
	OnRevReg(OUT_AB,75,OUT_REGMODE_SYNC);
	Wait(time);
	Float(OUT_AB);
	Release(motorMutex);
}

void reculerDist(int distance){
	int time;
	time=(distance*10*1000/SPEED);
	reculerTime(time/2);
	Float(OUT_AB);
	// TODOBLUETOOTH SEND DIST SPEED
}

									// *********** TOURNER *********** //
void turnright(){
	Acquire(motorMutex);
	OnFwd(OUT_B,30);//30
	OnRev(OUT_A,30);
	Release(motorMutex);
}

void turnleft(){
	Acquire(motorMutex);
	OnFwd(OUT_A,30);
	OnRev(OUT_B,30);
	Release(motorMutex);
}

void turnImprecis(int angle){
		// Modulo 360
		Acquire(motorMutex);

		if (angle>=360) angle = angle - 360;
		else if (angle<=-360) angle = angle + 360 ;
		else if (angle<0) angle = angle + 360 ;

		if (angle>=0){ 
			if (angle>180){
				OnFwd(OUT_A,30);
				OnRev(OUT_B,30);
				//turnleft();
				angle= 360 - angle;
			}
			else { 
				OnFwd(OUT_B,30);//30
				OnRev(OUT_A,30);
				//turnright();
			}
		}

	float timeToTurn;
	timeToTurn=abs(angle)*1000/DEGPSEC;
	
	if (timeToTurn<=20) timeToTurn=30;

	NumOut(0,LCD_LINE1,angle);
	Wait(timeToTurn); // Temps à tourner calcule a partir de la vitesse de rotation

	Float(OUT_AB);
	Release(motorMutex);
}

void turn(int angle, int precision){
	int angleObjectif;
	int angleActuel;
	int iterationMax = 0;
	bool optimal = false;
	if (precision<=10) optimal = true;
	NumOut(0,LCD_LINE1,optimal);
	int angleInitial = getAngle();
	angleObjectif = (angleInitial + angle) ;

		if (angleObjectif>=360) angleObjectif = angleObjectif - 360;
		else if (angleObjectif<=-360) angleObjectif = angleObjectif + 360 ;
		//else if (angleObjectif<0) angleObjectif = angleObjectif + 360 ;

	int angleATourner=angle;
	int diff=angle;
	int abs_diff = abs(diff);

	while (abs_diff>precision && iterationMax<5){
		iterationMax+=1;
		ClearScreen();
		turnImprecis(angleATourner);
		angleActuel=getAngle();
		diff=(angleObjectif - angleActuel);

		if (abs(diff)<6 && abs(diff)>3){
			diff = diff /2 ;
		}
		
		if (diff>=360) diff = diff - 360;
		else if (diff<=-360) diff = diff + 360 ;
		else if (diff>=-360 && diff<=-350) diff = diff + 360 ;

		angleATourner=diff;
		abs_diff=abs(diff);
		NumOut(0,LCD_LINE2,diff);
		NumOut(0,LCD_LINE3,angleActuel);
		NumOut(0,LCD_LINE4,angleObjectif);
		Wait(150);
	}
}

void orienterVersAngle(int angleFinal){
	int angleActuel;
	int angleATourner;

	angleActuel=getAngle();
	angleATourner=angleFinal-angleActuel;
	if (angleATourner<0) angleATourner+=360;
	angleAEnvoyer=angleFinal- directionInitiale; 
	if (angleAEnvoyer<0) angleAEnvoyer+=360;
	if (leader) angleAnnonce=angleAEnvoyer; //TODOBLUETOOTH
	turn(angleATourner,2);

	angleASuivre=getAngle()-directionInitiale;
	if (angleASuivre<0) angleASuivre+=360;
}

inline void contournerObstacle(){
	int angleDirection;
	angleDirection = Random(100)*1.8 + 90;
	if (Random(100)>50) angleDirection=-angleDirection;
	if (!attrapee) close();
	if (!attrapee) release();
	reculerDist(5);
	obstacle=false;
	turn(angleDirection,10);
	angleAEnvoyer=angleDirection-directionInitiale;
	if (angleAEnvoyer<0) angleAEnvoyer+=360;
	if (leader){
		angleAnnonce=angleAEnvoyer; // TODOBLUETOOTH angle a envoyer
		angleASuivre=getAngle()-directionInitiale;
		if (angleASuivre<0) angleASuivre+=360;
	}
}

void orienterParRapportDepart(int angle){
	until (waitInfo==false){
		Wait(500);
	}
	int directionAAvoir;
	directionAAvoir=directionInitiale+angle;
	if (directionAAvoir>360) directionAAvoir-=360;
	orienterVersAngle(directionAAvoir);
	angleAnnonce=angle;
}
							// ******** VERIFICATIONS SUR LES OBSTACLES ********* //


bool verifierNonBalle(){
	bool ballePresente;
	turnImprecis(10);
	if (SensorUS(IN_2)<=NEAR){
		ballePresente=false;
		turnImprecis(-10);
	}
	else{
		ballePresente=true;
		turnImprecis(-15);
	}
	return ballePresente;
}

bool verifierNonRobot(){
	bool robotPresent;
	Float(OUT_AB);
	if (SensorUS(IN_2)>=NEAR){
		robotPresent=false;
	}
	else{
		turnImprecis(10);
		if (SensorUS(IN_2)<=NEAR){
			robotPresent=false; // C'est un Mur
			obstacle=true;
		}
		else{
			robotPresent=true;
		}
		turnImprecis(-10);
	}
	return robotPresent;
}


										// *********** DEPLACEMENT ET ACTIONS *********** //

void deplacerRobot(int distance){
	obstacle=false;
	bool ballePresente;
	bool robot;

	if (SensorUS(IN_2)>NEAR) avancerDist(distance);
	
	else{
		TextOut(0,LCD_LINE1,"obstacle");
		Wait(1000);
		ClearScreen();
		// TODOBLUETOOTH sendCancelMessage
		//easySendCancel(8);
		obstacle=true;
	// APRES AVOIR RENCONTRE UN OBSTACLE 
		// Suiveur : verifier si c'est un robot ou un mur
		// if (!leader){
		// 	robot=verifierNonRobot(); // On attend 2 sec pour voir si l'obstacle s'est déplacé (robot)
		// 	if (robot){
		// 		Wait(2000);
		// 	}
		// }
		// Leader : verifier si c'est la balle ou un mur
		// if (leader){
		// 	ballePresente=verifierNonBalle();
		// 	if (!ballePresente){
		// 		contournerObstacle();
		// 	}
		// 	else {
		// 		avancerDist(15);
		// 		obstacle=false;
		// 	}
		// }
		contournerObstacle();
	}
}

void deplacerRobotParBond(int distance){
	angleASuivre=getAngle();
	int i = 0;
	TextOut(0,LCD_LINE1,"Debut bond");
	Wait(1000);
	ClearScreen();
	while (i<20){
		if (SensorUS(IN_2)>=NEAR){
			deplacerRobot(distance);
		}
		i++;
	}
	TextOut(0,LCD_LINE1,"fin par bond");
	Wait(1000);
	ClearScreen();
}

void retourMaison(){
	int areneWidth = 200;
	int angleRetour;
	angleRetour=180+directionInitiale;
	if (angleRetour>360) angleRetour-=360;
	orienterVersAngle(angleRetour);
	TextOut(0,LCD_LINE4,"Face au mur");
	
	deplacerRobotParBond(10);
	reculerDist(5);
	turn(90,3);
	int distanceToRightWall = SensorUS(IN_2);
	turn(180,3);
	if ((distanceToRightWall)>10){
		distanceToRightWall-=10;
	}
	avancerDist(areneWidth/2-distanceToRightWall);
	turn(90,3);
	release();
	StopAllTasks();
}

int balleAutour(){
	int directionBalle;
	int distanceMin;
	int angleInitial;
	int angleDejaTourne;
	int distancePrecedente;
	int distance;
	int ecart;
	int tamponDirectionBalle;
	int closestWall;
	int seuilBalle;
	int angleMurRencontre;

	int premiereDistance;
	int secondeDistance;

	angleInitial=getAngle();
	distancePrecedente=100;
	directionBalle=1000;
	closestWall=100;
	seuilBalle=6;
	secondeDistance=closestWall;

	//easySendWait(2000);
	turn(-20,5);
	while (angleDejaTourne<160 && !attrapee){ // Tant que le demi-tour n'est pas complet
		Wait(100);
		distance=SensorUS(IN_2);
		ecart=distancePrecedente-distance;
		
		if ( ((abs(ecart)>=seuilBalle) && (distance<=40)) || (distance<=minDistanceBalle)) { 	// distance différente que précédemment 
			if (!((angleDejaTourne-angleMurRencontre <= 25) && ( distance>=closestWall*0.8 && distance<=closestWall*1.2 ))){ 
				closestWall=100;
					tamponDirectionBalle=getAngle();
					turn(18,5);	
					Wait(100);
					premiereDistance=SensorUS(IN_2);
								 // On tourne pour vérifier
					if (abs(premiereDistance-distance)>=5 || abs(premiereDistance-distance)<=2 || premiereDistance>=40){ // distance plus élevée => c'était peut etre la balle ou distance tres proche
						turn(10,4);
						Wait(100);
						ballePeutEtreTrouvee=true;
						directionBalle=tamponDirectionBalle;
						secondeDistance=(SensorUS(IN_2));
						if (abs(secondeDistance-distance)<5){ // C'était un mur !
							angleMurRencontre=angleDejaTourne;
							closestWall=secondeDistance;
						}
						else{ // C'était bien la balle
							directionBalle=tamponDirectionBalle;
							ballePeutEtreTrouvee=true;
							minDistanceBalle=distance;
							break;
						}
					}
					else {
						angleMurRencontre=angleDejaTourne;
						closestWall=distance;
						turn(-10,3);
					}
			}
		}
		turnImprecis(7);
		angleDejaTourne+=7;
		distancePrecedente=distance;
	}
	if (ballePeutEtreTrouvee){
		orienterVersAngle(directionBalle);
		approcheFurtive(10);
		Wait(150);
		if (SensorUS(IN_2)>=minDistanceBalle+3){
			turnImprecis(5);
			Wait(150);
			if (SensorUS(IN_2)<=minDistanceBalle){
				minDistanceBalle=SensorUS(IN_2);
				directionBalle=getAngle();
			}
			else{
				turnImprecis(-10);
				Wait(150);
				if (SensorUS(IN_2)<=minDistanceBalle){
					minDistanceBalle=SensorUS(IN_2);
					directionBalle=getAngle();
				}
			}
		}
		orienterVersAngle(directionBalle);
		turn(-5,3);
		approcheFurtive(minDistanceBalle+10);
		// TODOBLUETOOTH 
		// sendDeplacementInfo distanceAEnvoyer=minDistanceBalle
		int angleToBeSend= directionBalle-directionInitiale;
		if (angleToBeSend<0) angleToBeSend+=360;
		angleAnnonce=angleToBeSend;
		easySendAction(minDistanceBalle,SPEED/3,directionBalle);
	}
	else{
		if (angleMurRencontre>=160){
			turnImprecis(15);
			int angleToBeSend= getAngle()-directionInitiale;
			if (angleToBeSend<0) angleToBeSend+=360;
			angleAnnonce=angleToBeSend;
			easySendAction(8,SPEED,angleToBeSend);
		}
	}
	return directionBalle;
}

void recherche(){
	int iCheckBall;
	iCheckBall=0;
	int angleBalle;

	if (leader && !attrapee && startRobot){
		avancerDist(40);
	} 

	while(leader && !attrapee && startRobot){
		while(iCheckBall<7  && !attrapee && startRobot){
			deplacerRobot(10);
			iCheckBall++;
		}
		if (attrapee) break;
		iCheckBall=0;
		angleBalle=balleAutour();
		if (attrapee) break;
		if (ballePeutEtreTrouvee){
			balleAutour();
			if (!attrapee) ballePeutEtreTrouvee=false;
			if (attrapee) break;
			//orienterVersAngle(angleBalle);
			if (!attrapee) turn(-7,2);
		}
	}

}

task deplacer(){
	while(true){
		if (startRobot && !attrapee){
			while(!leader && startRobot){
				// Suivre instrutions BT
				// Envoyer instructions BT au suivant
			}
			if (!positionDonnee){ // Recherche au hasard
				obstacle = false;
				if (!attrapee) recherche();
				if (attrapee){
					// TODOBLUETOOTH envoyerInfosLeaderShip();
					Wait(1000);
					easySendLead();
					easySendLead();
					easySendLead();
					retourMaison();
				}
			}
			else { // Position de la balle donnée
				avancerDist(targetY-y);
				avancerDist(5);
				if (x>=targetX){
					turn(-90,2);
					avancerDist(x-targetX+1);
				}
				else {
					turn(90,2);
					avancerDist(targetX-x+1);
				}
				if (attrapee){
					turn(180,2);
					avancerDist(x-targetX);
					turn(90,2);
					deplacerRobotParBond(8);
				}
				while(!attrapee){
					recherche();
				}
				if (attrapee){
					Float(OUT_AB);
					retourMaison();
				}
			}
		}
		else if (attrapee){
			retourMaison();
		}
	}
}

task communicate(){
TextOut(0,LCD_LINE1,"BEGINNING");
	    
	    if (BluetoothStatus(CONNECTION) != NO_ERR)
	        StopAllTasks();
	    else
	        TextOut(0, LCD_LINE1, "Conn Success! ");

	while(true)
	{
		byte message[];

	    // Wait for START message
	    
	    until (getMessage(message))
	    {
	    	Wait(100);
	    }

	        	if(message[3] == intToByte(ourID)) // Because ourId (int)
		  		{	
				  	switch(message[4]) // Check du type de message 
					{

					///////// CALL FUNCTIONS |  WARNING :  ID = (short)(idMessage+1) I don t know if it is right /////////		
						
						case MSG_ACTION: // +1

							respondAction(message);
							if(idNext == 255)
							{
								// WE ARE LAST ONE
								actionExecution = true;
								sendAck(idMessage++,intToByte(ourID),intToByte(idPrevious),(idMessage),0);
								// TO DO ACTION
								ClearScreen();	
								TextOut(0,LCD_LINE1,"ACTION 0");
								NumOut(0,LCD_LINE2,angleReceived);
								NumOut(0,LCD_LINE3,distanceReceived);
								NumOut(0,LCD_LINE4,speedReceived);
								Wait(2000);
								ClearScreen();


								orienterParRapportDepart(angleReceived);
								oldOrderDistance = distanceReceived;

								distanceAvantCancel= avancerDistSpeed(distanceReceived,speedReceived);
							}
							else
							{
								// WE AREN T LAST ONE
								actionExecution = true;
								sendAction(idMessage++,intToByte(ourID),intToByte(idNext),(angleReceived),intToByte(distanceReceived),(speedReceived));
								// TO DO ACTION 

								ClearScreen();	
								TextOut(0,LCD_LINE1,"ACTION 0");
								NumOut(0,LCD_LINE2,angleReceived);
								NumOut(0,LCD_LINE3,distanceReceived);
								NumOut(0,LCD_LINE4,speedReceived);
								Wait(2000);
								ClearScreen();
								orienterParRapportDepart(angleReceived);
								oldOrderDistance = distanceReceived;
								distanceAvantCancel = avancerDistSpeed(distanceReceived,speedReceived);
							}
							actionExecution = false;
							break;

						case MSG_ACK: // + 1
							respondAck(message);
							break;

						case MSG_LEAD: // +1
							respondLead(message);
							leader = true;
							sendAck(idMessage++,intToByte(ourID),message[2],(idMessage),0); // I don t know if necessary
							break;

						case MSG_START: // +1

							if(respondStart(message))
							{
								// sendAck((idMessage+1),intToByte(ourID),message[2],(idMessage),0);
								// NOTHING TO DO
								startRobot=true;
							}
							else
							{
								// sendAck(idMessage,intToByte(ourID),message[2],(idMessage),1);								startRobot=true;
								startRobot=true;
							}
							break;

						case MSG_STOP: // +1
							respondStop(message);
							// TO DO : STOP ROBOT
							startRobot=false;
							//StopAllTasks();
							break;

						case MSG_WAIT: // +1
							respondWait(message);
							// TO DO wait : delayReceived
							waitInfo=true;
							Wait(delayReceived*1000);
							waitInfo=false;
							break;

						case MSG_CUSTOM: // + 0
							respondCustom(message);
						break;	

						case MSG_KICK: // +1
							respondKick(message);
							if (rankKickOut == rank) 
							{
								// TO DO : QUIT
								startRobot=false;
								StopAllTasks();
							}
							break;	

						case MSG_CANCEL: // +1
							respondCancel(message);
							if(actionExecution)
							{
								droitAvancer=false;
								if (distanceAvantCancel>=distanceCanceled) droitAvancer=true;// Plus besoin d'avancer
								else {
									oldOrderDistance=distanceCanceled- distanceAvantCancel;
									droitAvancer=true;
									distanceAvantCancel = avancerDistSpeed(oldOrderDistance,SPEED/10);
								}
								if(intToByte(idNext) == 0xFF)
								{
									actionExecution = true;
									sendAck(idMessage++,intToByte(ourID),intToByte(idPrevious),(idMessage),0);
									
								}
								else
								{
									actionExecution = true;
									sendAction(idMessage++,intToByte(ourID),intToByte(idNext),(angleReceived),intToByte(distanceCanceled),(speedReceived));
								} 
							}
							else
							{
								// ACTION NOT IN MOTION
								// TO DO : I DONT KNOW
							}
						break;																							

						default:
						TextOut(0, LCD_LINE6, "No Message");
						break;
					}
				}	
	ClearScreen();
  	}
}

// ******************************************** PROGRAMME PRINCIPAL ******************************************** //

task main(){
	// Initialisation des capteurs 
	SetSensorColorFull(IN_1); //Couleur
	SetSensorLowspeed(IN_2); //Sonar
	SetSensorLowspeed(IN_3); //Magnetique
	SetSensorTouch(IN_4); //Tactile

	directionInitiale=getAngle();
	Precedes(catch,deplacer,communicate);
}