/*******************************************
 *   Copyright (C) 2008 by Mauro Rodrigues *
 *******************************************/
#include "message.h"
#include "rs232.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};
	char temp[50], aux[10];

	//construcao da menssagem de teste
	strcpy(temp,"TESTECOM: Message to send: ");
	msg_tx[0]=MESSAGE_TEST;

	for(i=1;i < USART_SEND_LENGTH-1;i++)
		msg_tx[i]=(rand()%255);

	strcpy(aux, "\0");
	for (i=0;i<USART_SEND_LENGTH-1;i++){
		sprintf(aux, "%d\t",int(msg_tx[i]));
		strcat(temp, aux);
	}
	strcat(temp,"\n");
	rslog(temp);

	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);	

	if (sendmessage(msg_tx, msg_rx) < 0){
		rslog("TESTCOM: sendmessage failed!\n");
		return -1; 
	}

	// Recepcao da messagem de teste e confirmacao
	strcpy(aux,"\0");
	strcpy(temp, "TESTECOM: Message received: ");
	for (i=0;i<USART_RECV_LENGTH-2;i++){
		if((msg_rx[i]!=msg_tx[i])){
			rslog("TESTCOM: Data packets doesn't match to the expected message!\n");
			return -1;
		}
		sprintf(aux, "%d\t", int(msg_rx[i]));
		strcat(temp, aux);
	}
	strcat(temp,"\n");
	rslog(temp);

	return 1;
}

int Message::readcanstate(void)
{

	unsigned char state,i;
	char aux[10], temp[50];
	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){
		rslog("READCANSTATE: sendmessage failed\n");
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]!=MESSAGE_SUCESS){
		rslog("READCANSTATE: Unknown error!\n");
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1]){
		rslog("READCANSTATE: Packet type is different from the source!\n");
		return -1;
	}

	strcpy(aux, "\0");
	strcpy(temp, "READCANSTATE: ");
	for(i=0;i<USART_RECV_LENGTH;i++){
		sprintf(aux,"\t%d",int(msg_rx[i]));
		strcat(temp, aux);
	}
	strcat(temp,"\n");
	rslog(temp);

	state=msg_rx[5];
	sprintf(temp,"\t%02X\t\n",state);
	rslog(temp);

	return 1;
}


int Message::readjoint(signed char *joint, unsigned char scu_id,unsigned char param)
{
	unsigned char state,i;
	char aux[10], temp[50];
	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){
		rslog("READJOINT: Sendmessage failed\n");
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]==MESSAGE_INVREQ){
		rslog("READJOINT: Invalid SCU ID!\n");
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1]){
		rslog("READJOINT: Packet type is different from the source!\n");
		return -1;
	}

	strcpy(aux, "\0");
	strcpy(temp, "READJOINT: ");
	for(i=2;i<USART_RECV_LENGTH-2;i++)
	{
		sprintf(aux, "\t%d", (signed int)((signed char)(msg_rx[i])));
		strcat(temp, aux);
		joint[i-2]=((signed char)(msg_rx[i]));
	}
	strcat(temp,"\n");
	rslog(temp);

	state=msg_rx[5];
	sprintf(temp,"\t%02X\t\n",state);
	rslog(temp);

	return 1;

}



int Message::readspecial(signed char *sensor, unsigned char scu_id)
{
	unsigned char i;
	char aux[10], temp[50];
	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){
		rslog("READSPECIAL: Sendmessage failed\n");
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]==MESSAGE_INVREQ){
		rslog("READSPECIAL: Invalid SCU ID!\n");
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1])
	{
		rslog("READSPECIAL: Packet type is different from the source!\n");
		return -1;
	}

	strcpy(aux, "\0");
	strcpy(temp, "READSPECIAL: ");
	for(i=2;i<USART_RECV_LENGTH-1;i++){
		sprintf(aux, "\t%d", int(msg_rx[i]));
		strcat(temp, aux);
		sensor[i-2]=msg_rx[i];
	}
	strcat(temp,"\n");
	rslog(temp);

	return 1;
}


int Message::readorient(signed char *sensor)
{
	unsigned char i, sensors;
	char aux[10], temp[50];
	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){
		rslog("READORIENT: Sendmessage failed\n");
		return -1; 
	}

	// verificacao da menssagem
	if(msg_rx[0]==MESSAGE_INVREQ){
		rslog("READORIENT: Unknown error!\n");
		return -1;
	}
	if (msg_rx[1]!=msg_tx[1]){
		rslog("READORIENT: Packet type is different from the source!\n");
		return -1;
	}

	strcpy(aux, "\0");
	strcpy(temp, "READORIENT: ");
	for(i=2;i<USART_RECV_LENGTH-1;i++){
		sprintf(aux, " \t%d", int (msg_rx[i]));
		strcat(temp, aux);
		sensor[i-2]=msg_rx[i];
	}
	strcat(temp,"\n");
	rslog(temp);

	return 1;
}


int Message::applyjoint(unsigned char scu_id,unsigned char param,signed char *servos)
{
	unsigned char i;
	char aux[10], temp[50];
	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){
		rslog("APPLYJOINT: The sendmessage failed!\n");
		return -1; 
	}

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

	return 1;	
}


int Message::applycontrol(unsigned char scu_id,unsigned char param,signed char *servos)
{
	unsigned char i;
	char aux[10], temp[50];
	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){
		rslog("APPLYCONTROL: The sendmessage fail\n");
		return -1; 
	}

	// Verificação dos parametros 
	if (msg_rx[0]==MESSAGE_INVREQ){
		sprintf(temp, "APPLYCONTROL: Invalid SCU ID = %d\n", int(scu_id));
		rslog(temp);
		return -1;
	}
	
	if (msg_rx[1]!=msg_tx[1])
		rslog("APPLYCONTROL: Type packet is different from the source!\n");

	for (i=0;i<3;i++)
		if(msg_rx[i+2]!=msg_tx[i+2])
			rslog("APPLYCONTROL: Data packets don't match the expected message!\n");

	strcpy(aux, "\0");
	strcpy(temp, "APPLYCONTROL: ");
	for (i=0;i<USART_RECV_LENGTH;i++){
		sprintf(aux, " \t%d", int(msg_rx[i]));
		strcat(temp, aux);
	}
	strcat(temp,"\n");
	rslog(temp);

	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; )
			if((n=port.writecom(msg_tx[i]))<0){
				rslog("SENDMESSAGE: Send error\n");
				status--;
			}else
				i++;

		// 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){
				rslog("SENDMESSAGE: Receive error!\n");
				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){
				rslog("SENDMESSAGE: Receive error\n");
				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;

	//Slave Controller Unit a considerar
	if(scu_id==0)
		scu_id=0;

	//Parametro a consultar/aplicar
	if(param==0)
		param=0;

	//Valores default nos campos de dados
	if(val==NULL)
		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); 
}