/**
* ===================================================================================================
*
*       \file  RobCommFunc.c 
*      \brief  Ficheiro que contém todas as funções necessárias para o programa RobCommClient
*
* ===================================================================================================
*/

#include <stdio.h>
#include <stdlib.h>
#include "RobCommClient.h"
#include "header.h"


/**
* @brief  Função para configurar a comunicação TCP_IP
*
* @param  Widget and user data
* @return sock
*/
int ConfigureComm(char servIP[20], in_port_t servPort)
{    
    int retVal;                                  //Generic return value for later use.
    struct sockaddr_in servAddr;                // Construct the server address structure
    
    /*robCOMM server needs this time to go back listening*/
    sleep(2);
    
    // Create a reliable, stream socket using TCP
    int sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
    if(sock < 0) ExitWithSystemMessage("socket() failed");
    
    memset(&servAddr, 0, sizeof(servAddr));     // Zero out structure   
    servAddr.sin_family = AF_INET;              // IPv4 address family
    
    // Convert string in address of server
    retVal = inet_pton(AF_INET, servIP, &servAddr.sin_addr.s_addr);
    if(retVal == 0) ExitWithUserMessage("inet_pton() failed", "invalid address string");
    else if(retVal < 0) ExitWithSystemMessage("inet_pton() failed");
    
    servAddr.sin_port = htons(servPort);        /*faz a conversao da porta do servidor*/
    
    // Establish the connection to the echo server
    retVal=connect(sock, (struct sockaddr *) &servAddr, sizeof(servAddr));
    if( retVal < 0) ExitWithSystemMessage("connect() failed");    
    return sock;
}


/**
* @brief  Função para ler o ficheiro com os dados que veem do sherlock 
*
* @param  Widget and user data
* @return void
*/
void GetDataSherlock(double** ArrayRob, RobValues* DataRob)
{    
//     printf("ArrayRob %p\n",ArrayRob);
//     printf("ArrayRob-> %p\n",*ArrayRob);

    FILE *fp;
    char **str,linha[1024];
    double ArrayObj[200][20];
    int i=0,f,k=0;
    
    double**ArrayLocal=(double**)malloc(200*sizeof(double*));
    for(i=0;i<200;i++)
	ArrayLocal[i]=(double*)malloc(20*sizeof(double));   


    fp=fopen("/home/luis/share_folder/ObjTrab.txt","r");
    
    if(! fp)
	perror("Erro na abertura do ficheiro");
  
    while(feof(fp)==0)
    {
	fgets(linha,1000, fp);
	i++;
    }

    fseek(fp,0,SEEK_SET);

    str=(char**)malloc(i*sizeof(char*));

    for(f=0;f<i;f++)
	str[f]=(char*)malloc(1024*sizeof(char));

    i=0;
    while(feof(fp)==0)
    {
	fgets(str[i],1020, fp);
// 	printf("l %d >> %s\n",i,str[i]);
	i++;
    }
			    
    while(k<(i-1))
    {
	sscanf(str[k],"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",&ArrayObj[k][0], &ArrayObj[k][1], &ArrayObj[k][2], &ArrayObj[k][3], &ArrayObj[k][4], &ArrayObj[k][5], &ArrayObj[k][6], &ArrayObj[k][7], &ArrayObj[k][8], &ArrayObj[k][9], &ArrayObj[k][10]);
// 	printf("ArrayObj >> %lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", ArrayObj[k][0], ArrayObj[k][1], ArrayObj[k][2], ArrayObj[k][3], ArrayObj[k][4], ArrayObj[k][5], ArrayObj[k][6]);
	
	ArrayRob[k][0] = (int)ArrayObj[k][1];	/*Valor de XX dos obj trabalho*/
	ArrayRob[k][1] = (int)ArrayObj[k][0];	/*Valor de YY dos obj trabalho*/
	ArrayRob[k][2] = ArrayObj[k][2];	/*Circularidade*/
	ArrayRob[k][3] = ArrayObj[k][3];	/*Elipse Axis Ratio*/
	ArrayRob[k][4] = ArrayObj[k][4]*180/3.14;	/*Angulo do maior eixo da elipse que contém o objecto de trabalho*/
	ArrayRob[k][5] = (int)ArrayObj[k][5];	/*Numero total de objectos*/
	ArrayRob[k][6] = (int)ArrayObj[k][7];	/*Centro XX do objecto preto*/    
	ArrayRob[k][7] = (int)ArrayObj[k][6];	/*Centro YY do objecto preto*/	
	ArrayRob[k][8] = (int)ArrayObj[k][8];	/*Largura em XX do  objecto glob*/
	ArrayRob[k][9] = (int)ArrayObj[k][9];	/*Largura em YY do  objecto glob*/					
	ArrayRob[k][10] = ArrayObj[k][10];	/*Factor de forma dos objectos de trabalho*/					
// 	printf("J %p\n",&(ArrayRob[k][0]));
//  	printf("ArrayRob(%d):  XObj:%lf\tYObj:%lf\tCirc:%lf\tElipsRatio:%lf\tTotObj:%lf\tXBlack:%lf\tYBlack:%lf\n",k,ArrayRob[k][0],ArrayRob[k][1],ArrayRob[k][2],ArrayRob[k][3],ArrayRob[k][4],ArrayRob[k][5],ArrayRob[k][6]);
	k++;
    }
    
    (*DataRob).SizeArrayFile=k;       

    int j, Max_Obj_Inclin, Max_Obj_Horizont;
    double Property_Obj_Inclinad=0, Property_Obj_Horizont=0;    
    
    //Se esta condicao for aceite sao combinadas as tres propriedades 
    if(DataRob->CombinacaoPropriedades==1)
    {	
	//Ordenação dos objectos de acordo com o factor de forma
	for( j=0; j<k; j++)
	{
	    for(i=0;i<11;i++)
	    {
		//Neste caso tem em conta o factor de forma
		ArrayRob[j][11] = ( ArrayRob[j][2] + ArrayRob[j][3] ) / 2 ; //ArrayRob[j][10]
		
		if( (ArrayRob[j][11] > Property_Obj_Inclinad) && (ArrayRob[j][10] > 0.1) )
		{
		    Property_Obj_Inclinad = ArrayRob[j][11];
		    Max_Obj_Inclin=j;		    
		}
		
		//Este caso apenas analisa a circularidade e o ElipAxisRatio do objecto (esta propriedade sobrepoe-se a anterior)
		ArrayRob[j][12] = ( ArrayRob[j][2] + ArrayRob[j][3] ) / 2;
		
		if( (ArrayRob[j][12] > Property_Obj_Horizont) && (ArrayRob[j][12]>=0.80) )
		{
		    Property_Obj_Horizont = ArrayRob[j][12];
		    Max_Obj_Horizont=j;		    
		}	    	    
	    }
	    printf("%lf\t%lf\t%lf\t=\t%lf\n", ArrayRob[j][2], ArrayRob[j][3], ArrayRob[j][10], ArrayRob[j][11]);
	    printf("k: %d\n", k);
	}
	
	if( (Property_Obj_Horizont>=0.80) && (k>1) )
	{
	    for(i=0;i<11;i++)
	    {		
		ArrayLocal[0][i] = ArrayRob[0][i];
		ArrayRob[0][i] = ArrayRob[Max_Obj_Horizont][i];
		ArrayRob[Max_Obj_Horizont][i] = ArrayLocal[0][i];
	    }
	    printf("Encontrou objecto horizontal...\n");
	}
	else if (Property_Obj_Horizont<0.80 && k>1)
	{
	    for(i=0;i<11;i++)
	    {		
		ArrayLocal[0][i] = ArrayRob[0][i];
		ArrayRob[0][i] = ArrayRob[Max_Obj_Inclin][i];
		ArrayRob[Max_Obj_Inclin][i] = ArrayLocal[0][i];
	    }
	    printf("Encontrou objecto num plano inclinado...\n");
	}
	    
	    
	printf("ArrayRob DEPOIS...\n");
	for( j=0; j<k; j++)
	{
	    for(i=0;i<11;i++)
	    {		
		printf("%2.2lf\t", ArrayRob[j][i]);
	    }
	    printf("\n");
	}
    }
    //Se esta condicao for aceite procura-se o objecto mais proximo para se seguir
    if( (DataRob->CombinacaoPropriedades==0) && (k>1) )
    {
	
	printf("Vou ver o objecto que esta mais próximo!!!\n");
	
	//Ordenação dos objectos de acordo com a proximidade ao centro
	double dif_X, dif_Y, distancia=1000, ModDist, JuncPropriedades;

// 	printf("ArrayRob Antes...\n");
// 	for( j=0; j<k; j++)
// 	{
// 	    for(i=0;i<11;i++)
// 	    {		
// 		printf("%2.2lf\t", ArrayRob[j][i]);
// 	    }
// 	    printf("\n");
// 	}

	for( j=0; j<k; j++)
	{	    
	    for(i=0;i<11;i++)
	    {			
		dif_X = AbsValue(ArrayRob[j][0] - 640/2);
		dif_Y = AbsValue(ArrayRob[j][1] - 992/2);				
		
		ModDist= sqrt( pow( dif_X , 2 ) + pow( dif_Y , 2 ) );
		
		JuncPropriedades = ( ArrayRob[j][2] + ArrayRob[j][3] ) / 2;
		
		if(JuncPropriedades > 0.5  && (ModDist < distancia))
		{		
		    Max_Obj_Inclin=j;		
		    distancia=ModDist;
		    printf("Mudei\n");
		}
	    }
// 	    printf("coord_X:\t%lf\tcoord_Y:\t%lf\tdistancia:\t%lf\tMax_Obj_Ind:\t%d\n", ArrayRob[j][0], ArrayRob[j][1], ModDist, Max_Obj_Inclin);
	    
	    for(i=0;i<11;i++)
	    {		
		ArrayLocal[0][i] = ArrayRob[0][i];
		ArrayRob[0][i] = ArrayRob[Max_Obj_Inclin][i];
		ArrayRob[Max_Obj_Inclin][i] = ArrayLocal[0][i];
	    }
// 	    printf("X:%lf\tY:%lf\tCirc:%lf\t=\tEAR:%lf\n", ArrayRob[j][0], ArrayRob[j][1], ArrayRob[j][2], ArrayRob[j][3]);
// 	    printf("k: %d\n", k);
	}
	
// 	printf("ArrayRob DEPOIS...\n");
// 	for( j=0; j<k; j++)
// 	{
// 	    for(i=0;i<11;i++)
// 	    {		
// 		printf("%2.2lf\t", ArrayRob[j][i]);
// 	    }
// 	    printf("\n");
// 	}
	    
    }
    
    
    (*DataRob).ObjGlobLarguraX = ArrayRob[0][9];
    (*DataRob).ObjGlobLarguraY = ArrayRob[0][8];
//     printf("Centro pneu:	%lf\tCentro do monte:	%lf\n",ArrayRob[0][0],ArrayRob[0][6]);
    (*DataRob).SentidoRotGarra = ArrayRob[0][6] - ArrayRob[0][0];
    (*DataRob).ElipAxisRatioBruto = ArrayRob[0][3];
    fclose(fp);	    

/*    printf("\n%lf\t%lf\t%lf\t=\t%lf\n", ArrayRob[Indice_Max][2], ArrayRob[Indice_Max][3], ArrayRob[Indice_Max][10], ArrayRob[Indice_Max][11]);
    printf("Indice_Max: %d\n", Indice_Max);*/
}


/**
* @brief  Função para fazer o seguimento dos objectos
*
* @param  Widget and user data
* @return void
*/
void TrackingObjects(double** ArrayRob, double** ArrayTemp, RobValues* DataRob)
{  
    int i;    
    if((DataRob->TrackingState==0)  &&  (ArrayRob[0][0]!=0)  &&  (ArrayRob[0][1]!=0))
    {	
	/*Valores aqui já chegam ordenados tendo em conta o rácio entre os eixos da elipse que contém o objecto*/
	for(i=0;i<10;i++)
	{
	    ArrayTemp[0][i]=ArrayRob[0][i];
	}
	
// 	(*DataRob).SizeArrayTemp=(*DataRob).SizeArrayFile;
// 	    printf("Indice aqui %d >> %lf\t%lf\n",i, ArrayTemp[i][0], ArrayTemp[i][1]);
	DataRob->TrackingState=1;
    }    
    else if((DataRob->TrackingState==1)  &&  (ArrayRob[0][0]!=0)  &&  (ArrayRob[0][1]!=0))
    {
	int MinDist=1000, diferencaX, diferencaY, distancia, Indice;
	
	for(i=0;i<DataRob->SizeArrayFile;i++)
	{
	    diferencaX = AbsValue(ArrayTemp[0][0] - ArrayRob[i][0]);
	    diferencaY = AbsValue(ArrayTemp[0][1] - ArrayRob[i][1]);
	    
	    distancia= sqrt( pow( diferencaX , 2 ) + pow( diferencaY , 2 ) );
// 	    printf("distancia:   %d\n",distancia);	    
	    if(distancia < MinDist)
	    {
		MinDist = distancia;
		Indice = i;	
	    }
	}
	
	if(MinDist < 250)
	{
	    for(i=0;i<10;i++)	
	    {
		ArrayTemp[0][i] = ArrayRob[Indice][i];
	    }
	    DataRob->LostState=0;
	}
	else
	{
	    printf("...................Perdi o objecto.............\n");
	    DataRob->LostState=1;
	    DataRob->LostObjectCount= DataRob->LostObjectCount + 1;
	    
	    if(DataRob->LostObjectCount==5)
	    {
		/*Inicia nova procura*/
		DataRob->TrackingState=0;
		DataRob->LostObjectCount=0;
	    }
	}
     }  
     
     
    if(ArrayRob[0][0]!=0 && ArrayRob[0][1]!=0)
    {
	
	(*DataRob).ObjTrabX = (640/2)-(int)ArrayTemp[0][0];		//diferença entre o centro e o objecto em  XX
	(*DataRob).ObjTrabY = (992/2)-(int)ArrayTemp[0][1];		//diferença entre o centro e o objecto em  YY
	(*DataRob).Roundness = ArrayTemp[0][2];				//Circularidade
	(*DataRob).ElipAxisRatio = ArrayTemp[0][3];			//Elipse Axis Ratio
	(*DataRob).Angle = ArrayTemp[0][4];				//Angulo dos objectos de trabalho
	(*DataRob).NTotObj = ArrayTemp[0][5];				//Numero total de objectos
	(*DataRob).ObjGlobX = (640/2)-(int)ArrayTemp[0][6];		//Centro XX do objecto preto
	(*DataRob).ObjGlobY = (992/2)-(int)ArrayTemp[0][7];		//Centro YY do objecto preto	    
	
    }else
    {
	(*DataRob).ObjTrabX = (int)ArrayRob[0][0];			//diferença entre o centro e o objecto em  XX
	(*DataRob).ObjTrabY = (int)ArrayRob[0][1];			//diferença entre o centro e o objecto em  YY
	(*DataRob).Roundness = ArrayRob[0][2];				//Circularidade
	(*DataRob).ElipAxisRatio = ArrayRob[0][3];			//Elipse Axis Ratio
	(*DataRob).Angle = ArrayRob[0][4];				//Angulo dos objectos de trabalho
	(*DataRob).NTotObj = ArrayRob[0][5];				//Numero total de objectos
	(*DataRob).ObjGlobX = (640/2)-(int)ArrayRob[0][6];		//Centro XX do objecto preto
	(*DataRob).ObjGlobY = (992/2)-(int)ArrayRob[0][7];		//Centro YY do objecto preto
	DataRob->TrackingState=0;
    }
    

    if(ArrayRob[0][0]==0 && ArrayRob[0][1]==0)
    {
	(*DataRob).NObjTrab=0;
	printf("Não há objectos para apanhar\n");
    }else
    {
	(*DataRob).NObjTrab=(*DataRob).SizeArrayFile;
    }
    
//     printf(">> XRobot:%d\nYRobot:%d\nNObjTrab:%d\nNTotObj:%d\n", DataRob->ObjTrabX, DataRob->ObjTrabY, DataRob->NObjTrab, DataRob->NTotObj);

}


/**
* @brief  Função para fazer uma aproximação inicial ao objecto em função da calibração
*
* @param  Widget and user data
* @return void
*/
void Aproximacao_Inicial_Objecto(int sock, RobValues* DataRob, int X_Value, int Y_Value)
{  	    
    int Xini, Yini;    
    //Movimentacao feita a partir da calibracao
    Xini= ( X_Value * DataRob->CalibValue_mm ) / DataRob->CalibValue_pixeis;
    Yini= ( Y_Value * DataRob->CalibValue_mm ) / DataRob->CalibValue_pixeis;	    	    
    
    if(DataRob->MovimentoAproxInicial==0)
    {
	printf("Faz aproximação inicial...!\n");
	SetSpeedRobot(sock, DataRob->VelocMedia);
	MovimentoRelativo( sock, Xini, Yini, 0, 0, 0, 0);  	    		
	SetSpeedRobot(sock, DataRob->VelocTrab);
	DataRob->MovimentoAproxInicial=1;
	DataRob->TrackingState=0;
	//Estabiliza a imagem
	sleep(1);
    }	
}


/**
* @brief  Função para seleccionar o valor para fazer aproximação ao objecto
*
* @param  Widget and user data
* @return void
*/
void Aproximacao_ao_Objecto(int ObjTrabX, int ObjTrabY,int* x_move, int* y_move)
{  	    
	    /*Movimentações no eixo dos XX*/
	    if ( (ObjTrabX > 15) && (ObjTrabX < 20) )
		*x_move=1; 
	    else if ( (ObjTrabX >= 20) && (ObjTrabX < 70) )
		*x_move=2; 
	    else if ( (ObjTrabX >= 70) && (ObjTrabX < 140) ) 
		*x_move=15; 
	    else if (ObjTrabX >= 140)   
		*x_move=50; 	    

	    else if ( (ObjTrabX < -15) && (ObjTrabX > -20) )
		*x_move=-1; 	    
	    else if ( (ObjTrabX <= -20) && (ObjTrabX > -70) )
		*x_move=-2; 
	    else if ( (ObjTrabX <= -70) && (ObjTrabX > -140) )
		*x_move=-15; 
	    else if ( ObjTrabX <= -140) 
		*x_move=-50; 	    
	    else
		*x_move=0; 

	    /*Movimentações no eixo dos YY*/
	    if ((ObjTrabY > 15) && (ObjTrabY < 20))
		*y_move=1; 	    
	    else if ((ObjTrabY >= 20) && (ObjTrabY < 70))
		*y_move=2; 
	    else if ( (ObjTrabY >= 70) && (ObjTrabY < 140) ) 
		*y_move=15; 
	    else if (ObjTrabY >= 140) 
		*y_move=50; 	    
	    
	    else if ( (ObjTrabY < -15) && (ObjTrabY > -20) )
		*y_move=-1; 	    
	    else if ( (ObjTrabY <= -20) && (ObjTrabY > -70) )
		*y_move=-2; 
	    else if ( (ObjTrabY <= -70) && (ObjTrabY > -140) )
		*y_move=-15; 
	    else if ( ObjTrabY <= -140) 
		*y_move=-50; 	    
	    else
		*y_move=0;   
}


/**
 * @brief  Função para inicializar o programa do robot
 *
 * @param  Widget and user data
 * @return void
 */
void InicializeRobotProgram(double** ArrayRob, double** ArrayTemp, RobValues* DataRob, int* sock)
{
    char servIP[20], continuar[20];
    
    DataRob->TrackingState=0;				/*Indica se o robot está a seguir algum objecto ou não*/
    DataRob->LostObjectCount=0;				/*Perdeu o objecto e está a tentar encontrar*/
    DataRob->LostState=0;				/*Perdeu o objecto que estava a seguir*/
    DataRob->MovimentoAproxInicial=0;
    DataRob->CalibValue_mm=90;
    DataRob->CalibValue_pixeis=230;
    DataRob->NovaTentativa=0;
    DataRob->CombinacaoPropriedades=1;
    
    DataRob->DistObjRefGarra=0;				//Distancia aos objectos em planos inclinados
    
    DataRob->VelocTrab=180;				/*Velocidade de trabalho*/
    DataRob->VelocVarrimento=120;			/*Velocidade de varrimento*/
    DataRob->VelocMedia=400;				/*Velocidade nos movimentos rápidos*/	
    DataRob->VelocRapida=1500;				/*Velocidade de aproximação*/
    
    DataRob->AproxObj=0;				/*Condição que indica se o sistema já fez aproximação ao objecto ou não*/
    shm_robEnv->MinRelativosFunc_ok=1;			/*Opção para aplicar algoritmo para encontrar minimos relativos ou não*/	
    shm_robEnv->limiteZ=-560;				/*Limita a altura do espaço de trabalho*/
    DataRob->RefTrabX=550;
    DataRob->RefTrabY=0;   
    DataRob->RefTrabZ= shm_robEnv->limiteZ + 250;
    shm_robEnv->veloc_robot=0;
    DataRob->N_PneusApanhados=0;
    
    printf("Wait for robot comunication...\n"); 
    strcpy(servIP,"192.168.0.230"); 
    in_port_t servPort=4900;     
    *sock= ConfigureComm(servIP,servPort);  

    printf("\nPrima enter para começar ...\t");		    
    scanf("%s",continuar);
    
    //Inicia todo o processo    
    GripperClose();   
        
    SetToolCenterPointer( *sock, 1, 0, 0, 360, 0, 0, 0);    
    //Define a velocidade dos varrimentos
    SetLinearSpeedRobot( *sock, DataRob->VelocVarrimento); 
    
    SetSpeedRobot(*sock, DataRob->VelocMedia);
    MovimentoAbsoluto( *sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);       
    //Estabiliza a imagem
    sleep(1.5);
    
    //Extrai pela primeira vez os dados do sherlock
    GetDataSherlock(ArrayRob, DataRob);    
    TrackingObjects( ArrayRob, ArrayTemp, DataRob);        
    printf("X: %d\tY: %d\tObjTr: %d\tTot: %d\n", DataRob->ObjTrabX, DataRob->ObjTrabY, DataRob->NObjTrab, DataRob->NTotObj);
}


/**
 * @brief  Função que derruba o objecto segundo YY
 *
 * @param  Widget and user data
 * @return void
 */
void DerrubaObjectoSegundoYY(int sock, RobValues* DataRob, int largura_x, int largura_y)
{
    int largura_mm, r4, r5, r6;
    char r1, r2, r3;  
    double x, y,z,rx,ry,rz;
    char var_y[30]="varrimento_y";
//     char continuar[30];
    //Desativa o calculo dos dois minimos relativos
    shm_robEnv->MinRelativosFunc_ok=0;

    printf("Faz varrimento segundo YY\n");	
    largura_mm=90*largura_y/(250*2);
    printf("Largura YY: %d\n", largura_mm);

    GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6);		    
    shm_robEnv->CentroObj[1] = y;	
    shm_robEnv->MinDistSensorObj = -1;		    		   		    
    int NovoX, NovoY;
    if( y > DataRob->RefTrabY )
    {
	SetSpeedRobot(sock, DataRob->VelocMedia);
	MovimentoRelativo(sock,50,-55,0,0,0,0);		
	SetSpeedRobot(sock, DataRob->VelocTrab);
	MovimentoRelativoLinear(sock,var_y,0,110,0,0,0,0);	
	
	//Espera pelo valor que esta a ser calculado			
	while(shm_robEnv->MinDistSensorObj == -1) usleep(1000);
	
	printf("Altura do objecto a derrubar:  %d\n",shm_robEnv->MinDistSensorObj);	
	
	// 15 é uma tolerancia para a garra não tocar no pneu ao descer
	NovoX = x + 90;		    		   		    
	NovoY = y + 40 + largura_mm;			
    }
    else
    {
	SetSpeedRobot(sock, DataRob->VelocMedia);
	MovimentoRelativo(sock,50,55,0,0,0,0);		    
	SetSpeedRobot(sock, DataRob->VelocTrab);
	MovimentoRelativoLinear(sock,var_y,0,-110,0,0,0,0);	
	
	//Espera pelo valor que esta a ser calculado			
	while(shm_robEnv->MinDistSensorObj == -1) usleep(1000);
	printf("Altura do objecto a derrubar:  %d\n",shm_robEnv->MinDistSensorObj);	
	
	// 15 é uma tolerancia para a garra não tocar no pneu ao descer
	NovoX = x + 90;		    		   		    
	NovoY = y - 40 - largura_mm;					    		    
    }
    SetSpeedRobot(sock, DataRob->VelocMedia);
    MovimentoAbsoluto( sock, NovoX, NovoY, DataRob->RefTrabZ-75, -180, -1, 0);             
    
    int CoordenadaZ = (shm_robEnv->MinDistSensorObj) - 110 -75 + 40;         
    printf("Vou descer até à coordenada z=... %lf...\n", z-75-CoordenadaZ);
    
    if( (z-75-CoordenadaZ) > shm_robEnv->limiteZ  &&  (z-CoordenadaZ) < z)
    {    
/*	printf("\nDescer ...\t");		    
	scanf("%s",continuar);*/
	
	MovimentoRelativo(sock,0,0,-CoordenadaZ,0,0,0);     		    
	
	SetSpeedRobot(sock, DataRob->VelocMedia);
	if( y > DataRob->RefTrabY )
	    MovimentoRelativo(sock,0,-130,10,0,0,0);
	else
	    MovimentoRelativo(sock,0,130,10,0,0,0);
	
	SetSpeedRobot(sock, DataRob->VelocMedia);   
	MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);
	SetSpeedRobot(sock, DataRob->VelocTrab);
	
	shm_robEnv->MinRelativosFunc_ok=1;
	DataRob->TrackingState=0;
	DataRob->MovimentoAproxInicial=0;
    }
    else
    {
	DataRob->TrackingState=0;			
	DataRob->AproxObj=0;		    	

	printf("\nERRO - Problema com as coordenadas em altura...!\nLocalização: Derruba em YY\n");
	MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);
    }
}		    


/**
 * @brief  Função que derruba o objecto segundo XX
 *
 * @param  Widget and user data
 * @return void
 */
void DerrubaObjectoSegundoXX(int sock, RobValues* DataRob, int largura_x, int largura_y)
{
    int largura_mm, r4, r5, r6;
    char r1, r2, r3;  
    double x, y,z,rx,ry,rz;
    char var_x[30]="varrimento_x";
//     char continuar[30];
    //Desativa o calculo dos dois minimos relativos
    shm_robEnv->MinRelativosFunc_ok=0;
    
    GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6);
    shm_robEnv->CentroObj[0] = x;			    
    shm_robEnv->MinDistSensorObj = -1;		    
    
/*    //Varrimento com Z=100
    if(z > 80)
	MovimentoRelativo(sock,0,0,-50,0,0,0);*/
    
    largura_mm=(90*largura_x/(250*2));
    printf("Largura XX: %d\n", largura_mm);
		    
    MovimentoRelativoLinear(sock, var_x, 110, 0, 0, 0, 0, 0);		    
    //Espera pelo valor que esta a ser calculado			
    while(shm_robEnv->MinDistSensorObj == -1) usleep(1000);
	    
    printf("Altura do objecto a derrubar:  %d\n",shm_robEnv->MinDistSensorObj);	
	    
    int NovoX;
    SetSpeedRobot(sock, DataRob->VelocMedia);
    //Os 95 é da diferenca entre coordenadas x do sensor e da garra e 15 é uma tolerancia
    if( (x+90) > DataRob->RefTrabX )		    
    {
	MovimentoRelativo(sock,40,0,0,0,0,0);
	NovoX= x + 95 + 30 + largura_mm;	
    }
    else
    {
	MovimentoRelativo(sock,-80,0,0,0,0,0);
	NovoX= x + 95 - 30 - largura_mm;		    		    
    }    
    MovimentoAbsoluto( sock, NovoX, y, DataRob->RefTrabZ-75, -180, -1, 0);            
    
    SetSpeedRobot(sock, DataRob->VelocMedia);
		    
    //O valor 75 tem a ver com a diferenca entre a altura do varrimento e a altura actual
    int CoordenadaZ = (shm_robEnv->MinDistSensorObj) - 110 -75 + 60;	    
    printf("vou descer até à coordenada:... %lf...\n", z-75-CoordenadaZ);
    
    if( ((z-75-CoordenadaZ) > shm_robEnv->limiteZ)  &&  ((z-CoordenadaZ) < z) ) 
    {		    
/*	printf("\nPausa ...\t");		    
	scanf("%s",continuar);*/
	
	MovimentoRelativo(sock,0,0,-CoordenadaZ,0,0,0);     		    
		
	if( (x+90) > DataRob->RefTrabX )
	    MovimentoRelativo(sock,-130,0,10,0,0,0);
	else
	    MovimentoRelativo(sock,130,0,10,0,0,0);
	
	SetSpeedRobot(sock, DataRob->VelocMedia);
	MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);
	SetSpeedRobot(sock, DataRob->VelocTrab);
	
	//Desativa o calculo dos dois minimos relativos
	shm_robEnv->MinRelativosFunc_ok=1;
	DataRob->TrackingState=0;
	DataRob->MovimentoAproxInicial=0;
    }
    else
    {
	DataRob->TrackingState=0;			
	DataRob->AproxObj=0;		    	

	printf("\nERRO - Problema com as coordenadas em altura...!\nLocalização: Derruba em XX\n");
	MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);
    }		    
}


/**
 * @brief  Função responsável por localizar em altura e apanhar os objectos
 *
 * @param  Widget and user data
 * @return void
 */
void ApanhaObjectoHoriz( int sock, RobValues* DataRob, int ObjDetectX, int ObjDetectY)
{
    int r4, r5, r6;
    char r1, r2, r3;  
    double x, y,z,rx,ry,rz;
    char var_x[30]="varrimento_x", var_y[30]="varrimento_y";
//     char continuar[30];
    int bordosPneu[5], profundidade=0, k;
    GetPosition(sock,&x,&y,&z,&rx,&ry,&rz,&r1,&r2,&r3,&r4,&r5,&r6);
    shm_robEnv->CentroObj[0] = x;
    shm_robEnv->CentroObj[1] = y;
    shm_robEnv->DistSensorObj[0] = -1;		    
                   
    MovimentoRelativoLinear(sock,var_x,110,0,0,0,0,0);	    		    
		    
    while(shm_robEnv->DistSensorObj[0] == -1)  usleep(1000);					    
    bordosPneu[0]=shm_robEnv->DistSensorObj[0];
    bordosPneu[1]=shm_robEnv->DistSensorObj[1];
    printf("Distancia do Sensor >> MinAnteriorX:%d MinPosteriorX:%d\n",bordosPneu[0],bordosPneu[1]);
    
    shm_robEnv->DistSensorObj[0] = -1;		    
    SetSpeedRobot(sock, DataRob->VelocMedia);
    MovimentoRelativo(sock,-65,55,0,0,0,0);
        
    MovimentoRelativoLinear(sock,var_y,0,-110,0,0,0,0);	    		    
			    
    while(shm_robEnv->DistSensorObj[0] == -1)  usleep(1000);			
    bordosPneu[2]=shm_robEnv->DistSensorObj[0];
    bordosPneu[3]=shm_robEnv->DistSensorObj[1];
    printf("Distancia do Sensor >> MinAnteriorY:%d MinPosteriorY:%d\n",bordosPneu[2],bordosPneu[3]);						
    
    int temp=0, media, count1=0, count2=0;			
	
    //Faz a média da altura dos bordos do pneu encontrados
    for (k=0;k<4;k++)
    {
	if(bordosPneu[k] != 10000)
	{
	    temp = temp + bordosPneu[k];
	    count1++;
	}
    }
    media=temp/count1;
    printf("media: %d\n",media);
    temp=0;
    //Apenas aproveita os valores dos bordos maiores que a média fazendo a média dos mesmos
    for(k=0;k<4;k++)
    {
	if( (bordosPneu[k] > media)  &&  (bordosPneu[k] != 10000) )
	{
	    temp=temp + bordosPneu[k];
	    count2++;
	    printf("Caso_2\n");
	}
    }
    
    profundidade=temp/count2;		    
    printf("Profundidade: %d\n",profundidade);
    
    SetSpeedRobot(sock, DataRob->VelocMedia);
    MovimentoAbsoluto( sock, ObjDetectX, ObjDetectY, DataRob->RefTrabZ-75, -180, -1, 0);    
				    
    int CoordenadaZ = profundidade - 120 + 25;    
    printf("Vou desver até à coordenada:...%lf ...\n",z-CoordenadaZ);
    
    if( ((z-CoordenadaZ) > shm_robEnv->limiteZ)  &&  ((z-CoordenadaZ) < z) )
    {		    		    	
// 	SetSpeedRobot(sock, DataRob->VelocTrab+100);
	MovimentoRelativo(sock,0,0,-CoordenadaZ,0,0,0);
	
// 	printf("...Passei agora por aqui :)...");				    
	GripperOpen();
	
	//Sobe para uma altura de seguranca
	SetSpeedRobot(sock, DataRob->VelocMedia);
	MovimentoRelativo(sock,0,0,150,0,0,0);

	SetSpeedRobot(sock, DataRob->VelocRapida);
	//Coloca o pneu no local pretendido		    
	MovimentoAbsoluto( sock, DataRob->RefTrabX-150+20*DataRob->N_PneusApanhados, DataRob->RefTrabY+500, DataRob->RefTrabZ-10, -180, -1, 0);	
	DataRob->N_PneusApanhados = DataRob->N_PneusApanhados + 1;
	
	MovimentoRelativo(sock,0,0,-50,0,0,0);					    
	
	GripperClose();		    		    		   		    
	sleep(1);
	MovimentoRelativo(sock,0,0,50,0,0,0);	    
	
	MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);
	sleep(1);
	DataRob->TrackingState=0;
	SetSpeedRobot(sock,100);
	
	DataRob->AproxObj=0;	
	DataRob->MovimentoAproxInicial=0;
    }
    else
    {
	DataRob->TrackingState=0;			
	DataRob->AproxObj=0;		    	
	printf("\nERRO - Problema com as coordenadas em altura...!\nLocalização: Algoritmo de preensão\n");
	MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);
    }
}


/**
 * @brief  Função responsável por apanhar objectos inclinados
 *
 * @param  Widget and user data
 * @return void
 */
void Apanha_Obj_Inclinado(int sock, RobValues* DataRob, double** ArrayRob, double** ArrayTemp, double** MatrizTransf)
{
    int XRefGarra, YRefGarra;    
    char continuar[20];
    //Nesta fase a garra está num plano inclinado e necessita fazer o tracking tendo em conta 
    //o referencial da garra
    DataRob->CombinacaoPropriedades=0;
    DataRob->TrackingState=0;

    char varrimento_null[30]="varrimento_null", varrimento_incl[30]="varrimento_inclinado";

    do
    {
	GetDataSherlock(ArrayRob, DataRob);    
	TrackingObjects( ArrayRob, ArrayTemp, DataRob);        		    			    
	Aproximacao_ao_Objecto(DataRob->ObjTrabX, DataRob->ObjTrabY, &XRefGarra, &YRefGarra);
	printf("XRefGarra: %d\tYRefGarra: %d\n",XRefGarra,YRefGarra);
	if( XRefGarra > 0  &&  XRefGarra < 30)
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X, 2, varrimento_null);
	else if( XRefGarra >= 30 )
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X, 20, varrimento_null);
	
	else if( XRefGarra < 0  &&  XRefGarra > -30)
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X, -2, varrimento_null);
	else if( XRefGarra <= -30)
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X, -20, varrimento_null);
	
	if (YRefGarra > 0  &&  YRefGarra < 30)
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y, -2, varrimento_null);	//O eixo YY está trocado
	else if (YRefGarra >= 30 )
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y, -20, varrimento_null);	//O eixo YY está trocado
	    
	else if (YRefGarra < 0  &&  YRefGarra > -30)
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y, 2, varrimento_null);	
	else if (YRefGarra <= -30 )
	    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y, 20, varrimento_null);	
	
    }while( (XRefGarra != 0) || (YRefGarra != 0) );

    printf("Vai aproximar...\n");		
    SetLinearSpeedRobot( sock, 500); 
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y_LINEAR, 10, varrimento_null);	        			    
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X_LINEAR, -10, varrimento_null);
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Z_LINEAR, 40, varrimento_null);		
    SetLinearSpeedRobot( sock, DataRob->VelocVarrimento);         
    
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X_LINEAR, 120, varrimento_incl);
    printf("\n\nVarrimento em XX........................................ %d\n",DataRob->DistObjRefGarra);
    int Dist_Ref_Garra_X = DataRob->DistObjRefGarra;    
    
    SetLinearSpeedRobot( sock, 500); 
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X_LINEAR, -60, varrimento_null);
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y_LINEAR, +60, varrimento_null);
    SetLinearSpeedRobot( sock, DataRob->VelocVarrimento); 
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y_LINEAR, -120, varrimento_incl);

    printf("\n\nVarrimento em YY........................................ %d\n",DataRob->DistObjRefGarra);
    int Dist_Ref_Garra_Y = DataRob->DistObjRefGarra;    
    
    SetLinearSpeedRobot( sock, 500); 
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_X_LINEAR, 25, varrimento_null);
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Y_LINEAR, +60, varrimento_null);
    SetLinearSpeedRobot( sock, DataRob->VelocVarrimento);         
    int Valor_Medio_Dist_Ref_Garra=0;	    
    
    if( Dist_Ref_Garra_X != 0  &&  Dist_Ref_Garra_Y != 0)
	Valor_Medio_Dist_Ref_Garra = (Dist_Ref_Garra_X + Dist_Ref_Garra_Y) / 2;
    else if( Dist_Ref_Garra_X != 0  &&  Dist_Ref_Garra_Y == 0)
	Valor_Medio_Dist_Ref_Garra = Dist_Ref_Garra_X;
    else if( Dist_Ref_Garra_X == 0  &&  Dist_Ref_Garra_Y != 0)
	Valor_Medio_Dist_Ref_Garra = Dist_Ref_Garra_Y;
    else if( Dist_Ref_Garra_X == 0  &&  Dist_Ref_Garra_Y == 0){
	Valor_Medio_Dist_Ref_Garra = 0;
	printf("\nNao obtive valor do sensor  !!\n");
    }
    printf("\nValor médio do sensor: %d\n",Valor_Medio_Dist_Ref_Garra);
    
    if( DataRob->DistObjRefGarra < 350 )
	DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Z_LINEAR, Valor_Medio_Dist_Ref_Garra, varrimento_null);		
    else{
	printf("ERRO - DistObjRefGarra muito grande...prima Ctrl para sair...\n");
	scanf("%s",continuar);
    }
    
    GripperOpen();
    
    //Sobe para uma altura de seguranca
    SetLinearSpeedRobot( sock, 500); 
    DeslocamentoRefGarra(sock, MatrizTransf, DataRob, EIXO_Z_LINEAR, -200, varrimento_null);	
    SetLinearSpeedRobot( sock, DataRob->VelocVarrimento); 
    //Ajusta novamente o Tool Center Poiter para para valores normais
    SetToolCenterPointer( sock, 1, 0, 0, 360, 0, 0, 0);
    
    SetSpeedRobot(sock, DataRob->VelocRapida);

    //Coloca o pneu no local pretendido		    
    MovimentoAbsoluto( sock, DataRob->RefTrabX-150+20*DataRob->N_PneusApanhados, DataRob->RefTrabY+500, DataRob->RefTrabZ-10, -180, -1, 0);	
    DataRob->N_PneusApanhados = DataRob->N_PneusApanhados + 1;
     
    MovimentoRelativo(sock,0,0,-50,0,0,0);	
    
    GripperClose();		    		    		   		    
    sleep(1);
    MovimentoRelativo(sock,0,0,50,0,0,0);	    

    MovimentoAbsoluto( sock, DataRob->RefTrabX, DataRob->RefTrabY, DataRob->RefTrabZ, -180, -1, 0);
    sleep(1);
    DataRob->TrackingState=0;
    SetSpeedRobot(sock,100);		
    
    //Coloca as condições iniciais porque vai apanhar um novo objecto
    DataRob->AproxObj=0;		
    DataRob->MovimentoAproxInicial=0;	//Só faz a aproximação com a calibração a primeira vez
    DataRob->TrackingState=0;		//Deixa de seguir o objecto depois de este ser apanhado
    DataRob->CombinacaoPropriedades=1;
}


/**
 * @brief  Função para abrir a garra
 *
 * @param  Widget and user data
 * @return void
 */

void GripperOpen()
{   
    printf("_Vai abrir garra !_\n");
    int i;        
    shm_env->type = ESTADO;
    shm_env->param1 = VALVO;
    shm_env->param2 = 1;
    for(i=0;i<=50000000;i++){}
    shm_env->type = ESTADO;
    shm_env->param1 = VALVC;
    shm_env->param2 = 0;
	
}

/**
 * @brief  Função para fechar a garra
 *
 * @param  Widget and user data
 * @return void
 */

void GripperClose()
{    
    printf("_Vai fechar garra !_\n");
    int i;    
    shm_env->type = ESTADO;
    shm_env->param1 = VALVC;
    shm_env->param2 = 1;
    for(i=0;i<=50000000;i++){}
    shm_env->type = ESTADO;
    shm_env->param1 = VALVO;
    shm_env->param2 = 0;        
}


/**
* @brief  Função para fechar a comunicação com o robot
*
* @param  Widget and user data
* @return void
*/
void Kill_RobotComm(int sock)
{    
    shmdt (shm_robEnv);
    /*Eliminar a Sharememory de recepção shm_robEnv*/
    if(shmctl(shm_id_robEnv, IPC_RMID, NULL)==0) 
    printf("Foi limpa a sharememory de shm_robEnv !\n");

    printf("Fecha o programa RobComm !!\n");  
    close(sock);
    exit(0);
}


void ExitWithUserMessage(const char *msg, const char *detail)
{
    fputs(msg, stderr);
    fputs(": ", stderr);
    fputs(detail, stderr);
    fputc('\n', stderr);
    exit(1);
}

void ExitWithSystemMessage(const char *msg)
{
    perror(msg);
    exit(1);
}