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

#include "header.h"
#include "RS232Comm.h"

/**
 * @brief  Função para terminar o programa filho por Ctrl C 
 * @param  Widget and user data
 * @return void
 */
void Kill_RS232Comm(int x)
{
	printf("Filho:Desligado por sigint\n");
	close(fd);
	
	shmdt (shm_rec);
 	/*Eliminar a Sharememory de recepção*/
	if(shmctl(shm_id_rec, IPC_RMID, NULL)==0) 
		printf("Foi limpa a sharememory de recepção !\n");

	exit(0);
}


/**
 * @brief  Função para terminar o programa filho por quit na interface
 * @param  Widget and user data
 * @return void
 */
void kill_filho_quit(int x)		
{
	printf("\nFilho:Desligado por quit na interface\n");
	close(fd);
	exit(0);    
}	

/**
 * @brief  Função para determinar o tempo decorrido  
 * @param  Widget and user data
 * @return void
 */
long tictoc(int stat)
{
    /*Declaração de variaveis*/
    long temp_s, temp_us, temp;
    
    static struct timeval t1;
    struct timeval t2;
    
    if (stat==0) //é um tic
    {
        /*Para utilizar a funcao null utilizar #include <stdio.h> */
        gettimeofday(&t1, NULL);
        return 0;
    }
    else 
    {
        gettimeofday(&t2, NULL);
        temp_s=(t2.tv_sec-t1.tv_sec)*1000;
        
        temp_us=(t2.tv_usec-t1.tv_usec)/1000;
        if(temp_us>=0)
        {
            temp=temp_s + temp_us;
        }
        else
        {
            temp=(temp_s-1000) + (1000-t1.tv_usec/1000) + (t2.tv_usec/1000); 
        }
        
        return temp;
    }
} 


/**
 * @brief  Função para configurar a porta RS232
 * @param  Widget and user data
 * @return void
 */
void RS232Config(void)
{
	struct termios options;

	tcgetattr(fd, &options);
	cfsetispeed(&options, B9600); /* Set the baud rates to 9600 */
	cfsetospeed(&options, B9600);

	/* Enable the receiver and set local mode */
	options.c_cflag |= (CLOCAL | CREAD);
	options.c_cflag &= ~PARENB; 
	options.c_cflag &= ~CSTOPB;
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8; 
	options.c_cflag &= ~CRTSCTS;
	/* Enable data to be processed as raw input */
	options.c_lflag &= ~(ICANON | ECHO | ISIG);
	options.c_cc[VMIN]=0;
	options.c_cc[VTIME]=10;

	/* Set the new options for the port */
	tcsetattr(fd, TCSANOW, &options);    
}


/**
 * @brief  Função para calcular os valores do sensor em mm 
 * @param  Widget and user data
 * @return void
 */
void DataConvert_mm(double dist_doub, double* Dist_Array,int count_exp)
{
    /*Cálculo da distancia em mm*/
    double exp=-1.047;				  
    double dist_cal =16291 * pow( dist_doub, exp );                    				    				   
   				    
    if((dist_doub==0) && (count_exp!=0))
    {
	Dist_Array[count_exp]=Dist_Array[count_exp-1];
    }
    else if(dist_doub!=0)
    {						
	Dist_Array[count_exp]=dist_cal;
    }
}

/**
 * @brief  Função que calcula a distancia do objecto ao sensor
 * @param  Widget and user data
 * @return void
 */
void CalcDistObj(double* CoordRobot, double* DataMedian, int count_exp, int ValorCentroObj, int* MinAnterior, int* MinPosterior, int* InclinaAnterior, int* InclinaPosterior)
{
    int i, k, IndiceCentro;
    printf("Vou fazer o calculo da dist ao sensor\n");
    //Deteta o indice correspondente ao centro do objecto
    for(i=0; i<count_exp; i++)
    {
	if( (ValorCentroObj-CoordRobot[i]) < 2 && (ValorCentroObj-CoordRobot[i]) > -2)
	{
	    IndiceCentro=i;
// 	    printf("Indice:%d  Centro:%lf\n",i,CoordRobot[i]);
	    break;
	}
    }
    
    //Calcula a distancia média central
    double DistMediaCentral, DistCentral=0, count=0;
    for(i=IndiceCentro-15; i<(IndiceCentro+15);i++)
    {
	DistCentral = DistCentral + DataMedian[i];
	count++;
    }
    
    DistMediaCentral=DistCentral/count;
//     printf("Valor DistMediaCentral:%lf\n",DistMediaCentral);
    
    /*Faz a procura do minimo relativo posterior*/
    int PicoPosterior=10000;
    int IndicePicoPosterior;
    for(k=IndiceCentro;k<count_exp;k++)
    {
	if((DataMedian[k] <= PicoPosterior) && DataMedian[k]< (DistMediaCentral-10))
	{
	    PicoPosterior=DataMedian[k];
	    IndicePicoPosterior=k;
	}
	if(DataMedian[k+1] > PicoPosterior)    
	    break;
    }
//     printf("Indice:%lf  Pico Posterior:%lf\n",CoordRobot[k],PicoPosterior);    
    
    /*Faz a procura do minimo relativo anterior*/
    int PicoAnterior=10000;
    int IndicePicoAnterior;
    for(k=IndiceCentro;k>0;k--)
    {
	if(DataMedian[k] <= PicoAnterior && DataMedian[k]< (DistMediaCentral-10))
	{
	    PicoAnterior=DataMedian[k];	    
	    IndicePicoAnterior=k;
	}
	if(DataMedian[k-1] > PicoAnterior)    
	    break;
	
    }
    printf("Pico Anterior:%d  Pico Posterior:%d\n",PicoAnterior,PicoPosterior);    
    
    /*Resolução do caso particular em que é detectato um minimo relativo errado*/
    if( (PicoAnterior!=10000)  &&  (PicoPosterior!=10000) )
    {
	printf("Estou dentro do if...\n");
	if( (PicoAnterior - PicoPosterior)> 30 )
	{
	    printf("pico anterior\n");
	    
	    int PicoAnteriorInicial = PicoAnterior;	    
	    for(k=IndicePicoAnterior;k>0;k--)
	    {
		if(DataMedian[k] < PicoAnterior)
		{
		    PicoAnterior=DataMedian[k];	    			
		}
		if( (DataMedian[k-1] > PicoAnterior)  && (DataMedian[k] < PicoAnteriorInicial) )    
		{
		    printf("indice: %d\n",k);
		    break;		    
		}
	    }			  	    
	}	
	else if( (PicoAnterior - PicoPosterior)< -30 )
	{
	    printf("pico posterior\n");
	    
	    int PicoPosteriorInicial = PicoPosterior;    
	    for(k=IndicePicoPosterior;k<count_exp;k++)
	    {
		if(DataMedian[k] < PicoPosterior)
		    PicoPosterior=DataMedian[k];			
		if(DataMedian[k+1] > PicoPosterior  &&  (DataMedian[k] < PicoPosteriorInicial))    
		    break;
	    }		
	}	
    }
    printf("Fiz calculo de distancia\n");
    *MinAnterior=PicoAnterior;
    *MinPosterior=PicoPosterior;

    //Faz a procura do minimo absoluto posterior
    int MinAbsPosterior=10000;    
    for(k=IndiceCentro;k<count_exp;k++)
    {
	if( DataMedian[k] <= MinAbsPosterior ) 
	{
	    MinAbsPosterior=DataMedian[k];
	}
    }

    //Faz a procura do minimo absoluto anterior
    int MinAbsAnterior=10000;    
    for(k=IndiceCentro;k>0;k--)
    {
	if( DataMedian[k] <= MinAbsAnterior ) 
	{
	    MinAbsAnterior=DataMedian[k];
	}
    }

    *InclinaAnterior = MinAbsAnterior;
    *InclinaPosterior = MinAbsPosterior;
}


/**
 * @brief  Função para calcular a média dos valores 
 * @param  Widget and user data
 * @return void
 */
void DataMedianFunc(double* Dist_Array, double* DataMedian, int count_exp)
{
	double media, inc=0;
	int b,i; 
	
	/*Fazer a media*/
	for (b=0;b<(count_exp);b++)
	{
	    if((b<4) | (b>count_exp-5))
	    {					    
		DataMedian[b]=Dist_Array[b];
	    }else
	    {
		for (i=-4;i<5;i++)
		{    
		    inc=inc+Dist_Array[b+i];
		    // printf(">>%lf\n",inc);						
		}				
			
		media = inc/9;
		DataMedian[b]=media;
		inc=0;
	    }
	    // 	printf("Array: %d %lf\n",b,DataMedian[b]);						
	}
}

/**
 * @brief  Função para gravar em ficheiro os dados do sensor de uma forma manual  
 * @param  Widget and user data
 * @return void
 */
void WriteFile(double* CoordRobot, double* Dist_Array, int count_exp)
{
	    FILE *fp;     				/*Variavel para escrita em ficheiro*/
	    int i;	    
	    printf("Grava em ficheiro!\n"); 		//Faz a escrita no ficheiro

	    fp=fopen("/home/luis/share_folder/Datafile.txt","w+");
	    if(! fp)
	    {
		printf("Erro\n");
		perror("Erro na abertura do ficheiro");
		exit(0);
	    }		                        
	    	    
	    for(i=0;i<count_exp;i++)
	    {                                                  
	    fprintf(fp,"%lf\t%lf\n",CoordRobot[i],Dist_Array[i]);					    
	    }
	    fclose(fp);
}



/**
 * @brief  Função que recebe os dados pela porta serie e calcula a distancia ate ao objecto
 * @param  Widget and user data
 * @return void
 */
void VarrimentoSensor(int* count_exp, double dist_doub)
{
	static int GetCoordRobBegin, count_aux;
	double Dist_Array[1024], DataMedian[1024],Tempo_Seg[1024], CoordRobot[1024];				
	
	if (shm_robEnv->RobotComm==OPEN_PNEU_OK)
	{
	    printf("Função: VarrimentoSensor...\nshm_robEnv->RobotComm:\t%d\n",shm_robEnv->RobotComm);	    
	}

	if ( (shm_robEnv->RobotComm==INI_TICTOC_X)  | (shm_robEnv->RobotComm==INI_TICTOC_Y) | (shm_robEnv->RobotComm == OPEN_PNEU_OK) )
	{	    	    	    
	    *count_exp=0;
	    count_aux=*count_exp;
	    
	    if (shm_robEnv->RobotComm==INI_TICTOC_X)  
		GetCoordRobBegin = shm_robEnv->data_int[0];            //Valor de xx pedido ao robot
	    else if (shm_robEnv->RobotComm==INI_TICTOC_Y)  
		GetCoordRobBegin = shm_robEnv->data_int[1];            //Valor de yy pedido ao robot	    
	    
	    shm_robEnv->RobotComm=APPEND_DATA;
	    tictoc(0);                        
	}
	else if( shm_robEnv->RobotComm==APPEND_DATA )
	{	    
	    int tempo_ms=tictoc(1);
	    Tempo_Seg[count_aux]=(double)tempo_ms/1000.0;

	    DataConvert_mm(dist_doub,Dist_Array,count_aux);

	    printf("Distancia: %d %lf\n",count_aux,Dist_Array[count_aux]); 
	    count_aux=*count_exp+1;
	    *count_exp=count_aux;
	}
	else if( (shm_robEnv->RobotComm==CLOSE_TICTOC_X) | (shm_robEnv->RobotComm==CLOSE_TICTOC_Y) )
	{
	    printf("Inicia calculos...\n");
	    int GetCoordRobEnd;
	    
	    if (shm_robEnv->RobotComm==CLOSE_TICTOC_X)
		GetCoordRobEnd = shm_robEnv->data_int[0];            //Valor de xx pedido ao robot
	    else if (shm_robEnv->RobotComm==CLOSE_TICTOC_Y)
		GetCoordRobEnd = shm_robEnv->data_int[1];            //Valor de yy pedido ao robot
    
	    int Diference=GetCoordRobEnd-GetCoordRobBegin;
	    printf("Begin:%d End:%d\n",GetCoordRobBegin,GetCoordRobEnd); 
	    
	    double IncrementoRob=(double)Diference/(double)count_aux;   
	    
	    double Coordenadas;

	    /*Ajuste devido ao desfasamento entre o censor e a garra*/	
	    if (shm_robEnv->RobotComm==CLOSE_TICTOC_X)
		Coordenadas=(double)GetCoordRobBegin+40.0;
	    else if (shm_robEnv->RobotComm==CLOSE_TICTOC_Y)
		Coordenadas=(double)GetCoordRobBegin;	    	    
	    
	    int i;
	    for(i=0;i<count_aux;i++)
	    {   
		CoordRobot[i] = Coordenadas;
		Coordenadas = Coordenadas + IncrementoRob;					    
	    }					

	    DataMedianFunc(Dist_Array, DataMedian, count_aux);
	    
	    int CentroObj, MinDistSensor;					
// 	    printf("shm_robEnv->CentroObj[0]: _%d_\n",shm_robEnv->CentroObj[0]);

	    if (shm_robEnv->RobotComm==CLOSE_TICTOC_X)
	    {
		CentroObj = shm_robEnv->CentroObj[0]+95;
		printf("TICTOC_X\n");
	    }
	    else if (shm_robEnv->RobotComm==CLOSE_TICTOC_Y)
	    {
		CentroObj = shm_robEnv->CentroObj[1];
		printf("TICTOC_Y\n");
	    }
	    	    	    
	    printf("Centro do objecto: %d\n", CentroObj);
	    
	    if(shm_robEnv->MinRelativosFunc_ok==1)	    
	    {
		/*calcula os dois minimos relativos do pneu*/
		int MinAnterior,MinPosterior, InclinaAnterior, InclinaPosterior;;						
		CalcDistObj(CoordRobot, DataMedian, count_aux, CentroObj, &MinAnterior, &MinPosterior, &InclinaAnterior, &InclinaPosterior);
		printf("Minimo anterior: %d\n",MinAnterior);
		printf("Minimo posterior: %d\n",MinPosterior);
		shm_robEnv->DistSensorObj[0] = MinAnterior;
		shm_robEnv->DistSensorObj[1] = MinPosterior;
		
		printf("Anterior (Inclinação): %d\n",InclinaAnterior);
		printf("Posterior (Inclinação): %d\n",InclinaPosterior);
		shm_robEnv->DistSensorObj[2] = InclinaAnterior;
		shm_robEnv->DistSensorObj[3] = InclinaPosterior;
		
	    }
	    else if(shm_robEnv->MinRelativosFunc_ok==0)
	    {
		/*calcula o minimo absoluto do pneu*/
		MinDistSensor = MinDistObj( DataMedian, count_aux);
		shm_robEnv->MinDistSensorObj = MinDistSensor;
	    }	    
	    	    
	    WriteFile(CoordRobot,DataMedian,count_aux);	
			    
	    shm_robEnv->RobotComm=WAIT;										
	}  	
	else if( shm_robEnv->RobotComm == CLOSE_PNEU_OK )
	{
	    //Esta pequena condição permite saber se foi agarrado algum pneu ou não  
	    int i;
	    double ValorMedio=0;
	    printf("Fiz o CLOSE_PNEU_OK ...\n");
	    
	    for(i=0;i<count_aux;i++)
	    {   
		ValorMedio = ValorMedio + Dist_Array[count_aux];
	    }					
	    ValorMedio = ValorMedio / count_aux;
	    printf("Valor da distancia ao pneu agarrado... %lf\n", ValorMedio);
	    
	    shm_robEnv->RobotComm=WAIT;		
	}
}


/**
 * @brief  Função que calcula a distancia minima do objecto ao sensor
 * @param  Widget and user data
 * @return void
 */
int MinDistObj(double* DataMedian, int count_exp)
{
    int i, MinDistObj=10000;
    printf("Vou calcular a distancia minima ao sensor...\n");
    /*Deteta o indice correspondente ao centro do objecto*/
    for(i=0; i<count_exp; i++)
    {
	if(DataMedian[i]<MinDistObj)
	{
	    MinDistObj=DataMedian[i];
	}
    }
    
    printf("Minimo global:%d\n",MinDistObj);    
    
    
    return MinDistObj;
}


/**
 * @brief  Função que conecta e desconecta a ligaçao e que permite fazer a escolha da porta serie
 * @param  Widget and user data
 * @return void
 */
void EstadoLigacao(ShmMsg *shm_env, int pid_p, int* Comunic_Estado)
{    
    
    
	if(shm_env->type==20)
	{
		if(shm_env->param1 == 40)
		{
			fd=open( UBS_device1 ,O_RDWR);  //| O_NDELAY
		}
		else if(shm_env->param1 == 41)
		{
			fd=open( UBS_device2 ,O_RDWR);  
		}
		else
		{
			fd=open( UBS_device0 ,O_RDWR);  
		}
		
		if(fd == -1)
		{
			shm_env->command = RS232;
			shm_env->type = 50;
			shm_env->param1 = 50;
			shm_env->param2 = 50;	
			
			if(kill(pid_p , SIGUSR1)==-1)
				perror("Erro na abertura da porta serie - kill\n");	
			
			*Comunic_Estado=2; 	/*Vai activar o continue no ciclo exterior*/
		}
		
		/*RS232 config*/
		RS232Config();

		shm_env->command = RS232;
		shm_env->type = 25;
		shm_env->param1 = 25;
		shm_env->param2 = 25;	
		
		if(kill(pid_p , SIGUSR1)==-1)
		{
		    perror("sinal para o pai\n");	
		}			
		*Comunic_Estado=1; 	
	}
	else if(shm_env->type==22)
	{
		close(fd);
		shm_env->command = RS232;
		shm_env->type = 27;
		shm_env->param1 = 27;
		    shm_env->param2 = 27;	
		
		if(kill(pid_p , SIGUSR1)==-1)
			perror("sinal para o pai\n");	
			
		*Comunic_Estado=0;		/*Vai activar o continue no ciclo exterior*/
	}
}