/***************************************************************************
 *   Copyright (C) 2007 by Daniel Baptista *
 ***************************************************************************/
#include "message.h"



int Message::testcom(void)
{
	unsigned char i;
	int bcc;
	unsigned char msg_tx[USART_SEND_LENGTH]={0,0,0,0,0,0};
	unsigned char msg_rx[USART_RECV_LENGTH]={0,0,0,0,0,0,0};

	//construcao da menssagem de teste
	cout << "TESTECOM: Message to send:" << endl;
	msg_tx[0]=MESSAGE_TEST;

	for(i=1;i < USART_SEND_LENGTH-1;i++)
	{
		msg_tx[i]=(rand()%255);
	}
	
	for (i=0;i<USART_SEND_LENGTH-1;i++)
	{
		cout <<  int (msg_tx[i]) << " \t " ;
	}
	cout <<" \t" << endl;

	for (i=0,bcc=0;i<USART_SEND_LENGTH-1;i++)
	{
		bcc+=msg_tx[i];
	}
	msg_tx[USART_SEND_LENGTH-1]=(unsigned char) (bcc%256);	

	// Envio da menssagem de teste
	cout << "Mensagem enviada!\n" << endl;
	if (sendmessage(msg_tx,msg_rx)<0)
	{
		cout << "TESTCOM: The sendmessage fail" << endl;
		return -1; 
	}
	cout << "Com sucesso!" << endl;
	cout << "TESTECOM: Message received: " <<endl;
	// Recepcao da messagem de teste e confirmacao
	for (i=0;i<USART_RECV_LENGTH-2;i++)
	{
		if((msg_rx[i]!=msg_tx[i]))
		{
			cout << "TESTCOM:Data packets doesn't match to the expected message!" << endl;
			return -1;
		}
		cout <<  int (msg_rx[i])<< "\t" ;
		
	}
	
	cout << "\t" <<endl;
	return 1;
}

int Message::readcanstate(void)
{

	unsigned char state,i;

	unsigned char msg_tx[USART_SEND_LENGTH]={0,0,0,0,0,0};
	unsigned char msg_rx[USART_RECV_LENGTH]={0,0,0,0,0,0,0};
	unsigned char operation =OP_READ_EXTBUFF;
	signed char servos[3]={0,0,0};
	

	// Contrucao da menssagem de envio
	buildmessage(msg_tx,operation,0,0,servos);

	if (sendmessage(msg_tx,msg_rx)<0)
	{
		cout << "READCANSTATE: The sendmessage fail" <<endl;
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]!=MESSAGE_SUCESS)
	{
		cout <<"READCANSTATE: Unknown error!" << endl;
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1])
	{
		cout <<"READCANSTATE: Type packet is different from the source!" << endl;
		return -1;
	}

	cout << "READCANSTATE: " ;
	for(i=0;i<USART_RECV_LENGTH;i++)
		cout <<" \t" << int( msg_rx[i] );
		
	state=msg_rx[5];
	

	printf("\t%02X\t\n",state);

	return 1;
}


int Message::readjoint(signed char *joint, unsigned char scu_id,unsigned char param)
{
	unsigned char state,i;

	unsigned char msg_tx[USART_SEND_LENGTH]={0,0,0,0,0,0};
	unsigned char msg_rx[USART_RECV_LENGTH]={0,0,0,0,0,0,0};
	unsigned char operation =OP_READ_SENSORS;
	signed char servos[3]={0,0,0};

	// Contrucao da menssagem de envio
	buildmessage(msg_tx,operation,scu_id,param,servos);
	
	if (sendmessage(msg_tx,msg_rx)<0)
	{
		cout << "READJOINT: The sendmessage fail" <<endl;
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]==MESSAGE_INVREQ)
	{
		cout << "READJOINT: Invalid SCU ID!" <<endl;
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1])
	{
		cout << "READJOINT: Type packet is different from the source!"<< endl;
		return -1;
	}

	cout << "READJOINT: " ;
	for(i=2;i<USART_RECV_LENGTH-2;i++)
	{
		cout << "\t" << (signed int)((signed char)(msg_rx[i]))  ;
		joint[i-2]=((signed char)(msg_rx[i]));
	}
	state=msg_rx[5];

	printf("\t%02X\t\n",state);

	return 1;

}



int Message::readspecial(signed char *sensor, unsigned char scu_id)
{
	unsigned char i;

	unsigned char msg_tx[USART_SEND_LENGTH]={0,0,0,0,0,0};
	unsigned char msg_rx[USART_RECV_LENGTH]={0,0,0,0,0,0,0};
	unsigned char operation =OP_READ_SENSORS;
	signed char servos[3]={0,0,0};

	// Contrucao da menssagem de envio
	buildmessage(msg_tx,operation,scu_id,PARAM_SPECIAL,servos);
	
	if (sendmessage(msg_tx,msg_rx)<0)
	{
		cout << "READSPECIAL: The sendmessage fail"<< endl;
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]==MESSAGE_INVREQ)
	{
		cout << "READSPECIAL: Invalid SCU ID!" << endl;
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1])
	{
		cout << "READSPECIAL: Type packet is different from the source!" << endl;
		return -1;
	}

	cout << "READSPECIAL: " ;
	for(i=2;i<USART_RECV_LENGTH-1;i++)
	{
		cout << "\t" << int( msg_rx[i] );
		sensor[i-2]=msg_rx[i];
	}
	cout << "\t" <<endl;

	return 1;

}


int Message::readorient(signed char *sensor)
{
	unsigned char i, sensors;

	unsigned char msg_tx[USART_SEND_LENGTH]={0,0,0,0,0,0};
	unsigned char msg_rx[USART_RECV_LENGTH]={0,0,0,0,0,0,0};
	unsigned char operation =OP_READ_SENSORS;
	signed char servos[3]={0,0,0};

	//sensors=(((unsigned char)SENSORS_ORIENTATION) << ((unsigned char) SENSORS_LOC)) & ((unsigned char)SENSORS_MASK);
	sensors=PARAM_SPECIAL;
	// Contrucao da menssagem de envio
	buildmessage(msg_tx,operation,0,sensors,servos);
	
	if (sendmessage(msg_tx,msg_rx)<0)
	{
		cout << "READORIENT: The sendmessage fail" << endl;
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]==MESSAGE_INVREQ)
	{
		cout << "READORIENT: Unknown error!" << endl ;
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1])
	{
		cout << "READORIENT: Type packet is different from the source!" << endl;
		return -1;
	}

	cout << "READORIENT: " << endl;
	for(i=2;i<USART_RECV_LENGTH-1;i++)
	{
		cout << " \t" << int (msg_rx[i])  ;
		sensor[i-2]=msg_rx[i];
	}
	cout << "\t" << endl;

	return 1;

}


int Message::applyjoint(unsigned char scu_id,unsigned char param,signed char *servos)
{
	unsigned char i;

	unsigned char msg_tx[USART_SEND_LENGTH]={0,0,0,0,0,0};
	unsigned char msg_rx[USART_RECV_LENGTH]={0,0,0,0,0,0,0};
	signed char operation =OP_APPLY_JOINT;


	// Contrucao da menssagem de envio
	buildmessage(msg_tx,operation,scu_id,param,servos);

		
	if (sendmessage(msg_tx,msg_rx)<0)
	{
		cout << "APPLYJOINT: The sendmessage fail" << endl;
		return -1; 
	}


	// Verificação dos parametros 
    	if (msg_rx[0]==MESSAGE_INVREQ)
	{
		cout << "APPLYJOINT: Invalid SCU ID = " << int(scu_id) <<endl;
		return -1;
	}
	
	if (msg_rx[1]!=msg_tx[1])
	{
		cout << "APPLYJOINT: Type packet is different from the source!" <<endl;
		return -1;
	}
	for (i=0;i<3;i++)
	{
		if(msg_rx[i+2]!=msg_tx[i+2])
		{
			cout << "APPLYJOINT:Data packets doesn't match to the expected message!" << endl;
			return -1;
		}
	}
	
	cout << "APPLYJOINT: ";
	for (i=0;i<USART_RECV_LENGTH;i++)
		cout <<"\t" <<int ( msg_rx[i] );
	
	cout << "\t" << endl;
	

	return 1;

		

}


int Message::applycontrol(unsigned char scu_id,unsigned char param,signed char *servos)
{

	unsigned char i;

	unsigned char msg_tx[USART_SEND_LENGTH]={0,0,0,0,0,0};
	unsigned char msg_rx[USART_RECV_LENGTH]={0,0,0,0,0,0,0};
	signed char operation =OP_APPLY_CONTROL;
	
	// Contrucao da menssagem de envio
	buildmessage(msg_tx,operation,scu_id,param,servos);
		
	if (sendmessage(msg_tx,msg_rx)<0)
	{
		cout << "APPLYCONTROL: The sendmessage fail" << endl;
		return -1; 
	}



	// Verificação dos parametros 
    	if (msg_rx[0]==MESSAGE_INVREQ)
	{
		cout << "APPLYCONTROL: Invalid SCU ID = " << int (scu_id) << endl ;
		return -1;
	}
	
	if (msg_rx[1]!=msg_tx[1])
		cout << "APPLYCONTROL: Type packet is different from the source!"<< endl;

	for (i=0;i<3;i++)
	{
		if(msg_rx[i+2]!=msg_tx[i+2])
		{
			cout <<"APPLYCONTROL: Data packets doesn't match to the expected message!" << endl;
		}
	}

	cout << "APPLYCONTROL: ";
	for (i=0;i<USART_RECV_LENGTH;i++)
		cout <<" \t" << int (msg_rx[i]) ;
	
	cout << "\t" << endl;

	return 1;

}

int Message::sendmessage (unsigned char *msg_tx,unsigned char *msg_rx)
{
	signed char tries=-1,tries_2=-1, max_tries=10;
	unsigned char i;
	signed int n,status;
	unsigned int bcc;
	//unsigned int j;
	
	SerialPort port;

	// Tipos de erros
	signed int err=0;

	do
	{
		err=0;
		if(tries > 0)
		{
			if (port.resetcom() < 0)
			{
				err=ERROR_RESETCOM;
			}	
		}

		// Envio da menssagem
		for (i=0,status=0;i<USART_SEND_LENGTH;)
		{
		
			//printf(" %d\t",msg_tx[i]);
			if((n=port.writecom(msg_tx[i]))<0)
			{
				cout << "SENDMESSAGE: Send error" <<endl;
				status--;
			}
			else
			{
				i++;
			}
		}

		//printf("\n");

		// Verificacao se a menssagem foi enviada
		if (status < 0)
		{
			tries++;
			err=ERROR_REQ;
			continue;
		}


		status=0;
		do
		{
			if((n=port.readcom(&msg_rx[0]))< 0)
			{
				cout << "SENDMESSAGE: Recive error " << endl;
				status--;
			}

		}while((msg_rx[0] < MESSAGE_MINTYPE) && (++tries_2) <= USART_RECV_LENGTH-1);

		if (status < 0 || tries_2==USART_RECV_LENGTH)
		{
			tries_2=0;
			tries++;
			err=ERROR_SOF;
			continue;
		}


		for (i=1,status=0;i<USART_RECV_LENGTH;i++)
		{
			if((n=port.readcom(&msg_rx[i])) < 0)
			{
				cout << "SENDMESSAGE: Recive error " << endl;
				status--;
			}
		}
		
		if (status < 0)
		{
			err=ERROR_EMPTYMSG;
			tries++;
			continue;
		}

		//for (i=0;i<USART_RECV_LENGTH;i++)
		//{
		//	printf(" %d\t",msg_rx[i]);
		//}
		//printf("\n");


		for (i=0,bcc=0;i<USART_RECV_LENGTH-1;i++)
		{
			bcc+=msg_rx[i];
		}
		
		bcc=((unsigned char) (bcc%256));

		// Erro de BCC na mensagem de resposta
        	if (bcc!=msg_rx[USART_RECV_LENGTH-1])
			err=ERROR_BCC_RECV;
        	// Erro de OpCode não coincidente
        	else if (msg_rx[1]!=msg_tx[1])
            		err=ERROR_OPCODE;
        	// Mensagem transmitida com parâmetros inválidos
        	else if (msg_rx[0]==MESSAGE_INVREQ)
        		err=ERROR_INVREQ;
        	// Mensagem transmitida com erro de BCC
        	else if (msg_rx[0]==MESSAGE_INVALID)
            		err=ERROR_BCC_SEND;
        	// Master não consegue ler a mensagem de solicitação
        	else if (msg_rx[0]==MESSAGE_ERROR)
            		err=ERROR_INV_SEND;
        	// Mensagem de natureza desconhecida
        	else if (msg_rx[0]!=MESSAGE_SUCESS && msg_rx[0]!=MESSAGE_TEST)
            		err=ERROR_INV_RECV;
	
	//printf("err= %d\n",err);
		
	}while(err != OK && (++tries)<=(max_tries-1));

	if (err != OK && max_tries>=7)
	{
		switch (err)
		{
			case ERROR_REQ:
				cout << "Mensagem de solicitação não transmitida!" <<endl;
				break; 
			case ERROR_SOF:
				cout << "Primeiro byte não detectado!" <<endl;
				break;
			case ERROR_EMPTYMSG:
				cout << "Impossivel obter resposta!" << endl;
				break;
			case ERROR_INV_RECV:
				cout <<"Mensagem de resposta inválida!" << endl;
				break;
			case ERROR_INV_SEND:
        	    		cout <<"Mensagem de solicitação não recebida com sucesso pelo Master!" << endl;
				break;
			case ERROR_BCC_RECV:
				cout <<"Mensagem de resposta com erro de BCC!" << endl;
				break;
			case ERROR_BCC_SEND:
				cout <<"Mensagem de solicitação com erro de BCC no Master!" << endl;
				break;
			case ERROR_OPCODE:
				cout <<"Byte de tipo (OpCode) não coincidente!" << endl;
				break;
			case ERROR_INVREQ:
				cout <<"Mensagem com parametros invalidos!" << endl;
				break;

			case ERROR_RESETCOM:
				cout <<"Erro na reinicialização dos buffers série" << endl;
				break;
			default :
				cout <<"Erro desconhecido!" << endl;
				break;
		}	
		return -1;
	}

	return 1;
}


int Message::buildmessage(unsigned char *message,unsigned char operation,unsigned char scu_id,unsigned char param,signed char *val)
{
	unsigned char operation_patt;
	unsigned char scuID_patt;
	unsigned char param_patt;
	unsigned char type;
	unsigned char i;
	unsigned int bcc;
	
	//if(operation==0)             //Tipo de mensagem default
	//	operation=OP_READ_EXTBUFF;
	
	if(scu_id==0)				//Slave Controller Unit a considerar
		scu_id=0;

	if(param==0)					//Parametro a consultar/aplicar
		param=0;
	
	if(val==NULL)					//Valores default nos campos de dados
	{
		for (i=0;i<USART_SEND_LENGTH-3;i++)
			val[i]=0;
	}
	
	// ===== Construção do pacote de tipo =====================================
	//Cálculo do padrão binário referente à operação 
	operation_patt =(operation << ((unsigned char) OPERATION_LOC)) & ((unsigned char)OPERATION_MASK);
	//Cálculo do padrão binário devido ao SCU a lidar
	scuID_patt = (scu_id << ((unsigned char)SCUID_LOC)) & ((unsigned char)SCUID_MASK);
	//Cálculo do padrão binário devido ao id do dispositivo
	param_patt = (param << ((unsigned char)PARAM_LOC))& ((unsigned char)PARAM_MASK);

	// Or entre os vários padrões 
	type = param_patt | scuID_patt | operation_patt;

	message[0]=(unsigned char)MESSAGE_REQ;
	message[1]=type;

	for (i=2;i<USART_SEND_LENGTH-1;i++)
	{	
		if(val[i-2]==0)
		{
			val[i-2]=0;
			message[i]=val[i-2];
		}
		else
			message[i]=val[i-2];
	}

	for (i=0,bcc=0;i<USART_SEND_LENGTH-1;i++)
	{
		bcc+=message[i];
	}
	message[USART_SEND_LENGTH-1]=(unsigned char) (bcc%256);	
	return (1); 

}