/****************************************************************************/
/* rhino.cpp (c++)                                                          */
/*--------------------------------------------------------------------------*/
/* Licencia GPL                                                             */
/* Implementacion de la Clase Rhino                                         */
/* Clase para la gestion del estado del controlador del Servo Robot XR4     */
/*                             RHINO                                        */
/****************************************************************************/
/* Marco Alvarez Reyna                                                      */
/* CIII UTN-FRC Argentina                                                   */
/* Mayo 2007 / Abril 2008                                                   */
/* mail: marcoalrey@gmail.com                                               */
/****************************************************************************/

#include "rhino.h"

Rhino::Rhino()
/***************************************************************/
/*  Constructor                                                */
/***************************************************************/
{
    /* Valores por defecto */
    uiErrorCode = 0; 
    uiStatusCode = 0; 
    uiLogLevel  = 3; 
    pFile       = NULL;

    InitMessageVectors(RhinoBotErrorMsg, RhinoClassErrorMsg, RhinoBotStatusMsg, RhinoClassStatusMsg);
}

Rhino::~Rhino()
/***************************************************************/
/*  Destructor                                                 */
/***************************************************************/
{
    pFile       = NULL;
}

int Rhino::set_LogLevel(unsigned int _uiLogLevel)
/***************************************************************/
/*  Setea el nivel de logueo de errores                        */
/***************************************************************/
/* LOG LEVEL: "unsigned int uiLogLevel"
               MAX_LOG_LEVEL

   0 - El mensaje de error se almacena en una variable de la clase.
   1 - Se muestran en consola los mensajes de error.
   2 - Se graban en un archivo los mensajes de error.
   3 - Los mensajes de error se graban en arch. y se muestran en consola.
*/
{
    if(_uiLogLevel < 0 || _uiLogLevel > MAX_LOG_LEVEL)
    {
	uiLogLevel = 3; //Default LogLevel
	ProcessErrorMsg(LOG_LEV);
	ProcessStatusMsg(RC_DEF_LOG_LEV);
	return -1;	
    }
    
    uiLogLevel = _uiLogLevel;
    return 0;
}

int Rhino::get_LogLevel(unsigned int *_uiLogLevel)
/***************************************************************/
/*  Devuelve el nivel de logueo de errores                     */
/***************************************************************/
{
    *_uiLogLevel = uiLogLevel;
    return 0; 
}

int Rhino::get_ErrorMsg(unsigned int _uiErrorCode, char *_cTextError)
/***************************************************************/
/*  Devuelve le mensaje de error corrspondiente al codigo de   */
/*  error requerido                                            */
/***************************************************************/
{
    /* Rhino Bot Errors */
    if(_uiErrorCode >= 0 && _uiErrorCode <= MAX_RB_ERROR_CODES)
    {
	strcpy(_cTextError, RhinoBotErrorMsg[_uiErrorCode]);      
	return 0;
    }

    /* Rhino Class Errors */
    if(_uiErrorCode > MAX_RB_ERROR_CODES && _uiErrorCode <= (MAX_RB_ERROR_CODES + MAX_RC_ERROR_CODES))
    {
	strcpy(_cTextError, RhinoClassErrorMsg[_uiErrorCode-MAX_RB_ERROR_CODES]);
	return 0;
    }
    
    ProcessErrorMsg(ERR_CODE);
    return -1;
}
int Rhino::get_ErrorMsg(char *_cTextError)
/***************************************************************/
/*  Devuelve le mensaje de error del ultimo error producido    */
/***************************************************************/
{
    /* Rhino Bot Errors */
    if(uiErrorCode >= 0 && uiErrorCode <= MAX_RB_ERROR_CODES)
    {
	strcpy(_cTextError, RhinoBotErrorMsg[uiErrorCode]);      
	return 0;
    }

    /* Rhino Class Errors */
    if(uiErrorCode > MAX_RB_ERROR_CODES && uiErrorCode <= (MAX_RB_ERROR_CODES + MAX_RC_ERROR_CODES))
    {
	strcpy(_cTextError, RhinoClassErrorMsg[uiErrorCode-MAX_RB_ERROR_CODES]);
	return 0;
    }
    
    ProcessErrorMsg(ERR_CODE);
    return -1;
}

int Rhino::ProcessErrorMsg(unsigned int _uiErrorCode)
/***************************************************************/
/*  Rutina de procesamiento de errores                         */
/***************************************************************/
/*  FUNCION PRIVADA                                            */
/***************************************************************/
{
    if((_uiErrorCode < 0) || (_uiErrorCode > (MAX_RC_ERROR_CODES + MAX_RB_ERROR_CODES)))
    {
	ProcessErrorMsg(ERR_CODE);
    }
    else
    {
	uiErrorCode = _uiErrorCode;

	get_ErrorMsg(cErrorMessage);
	sprintf(RhinoMessages, "RhinoClass->ProcessErrorMsg:\n\tUltimo cmd enviado: %s\n\tUltima cadena recibida: %s\n\tCodigo de error: %d\n\tLog Level: %d\n\tMSG: %s\n", LastCmdAndMsg.LastCommandtoRhino, LastCmdAndMsg.LastMessageFromRhino, uiErrorCode, uiLogLevel, cErrorMessage);

	if(uiLogLevel == 0)
	{
	    LogMessage(LMOCBk, RhinoMessages);
	}
	if(uiLogLevel == 1)
	{
	    LogMessage(LMODisplay, RhinoMessages);
	}
	if(uiLogLevel == 2)
	{
	    LogMessage(LMOFile, RhinoMessages);
	}
	if(uiLogLevel == 3)
	{
	    LogMessage(LMODisplay, RhinoMessages);
	    LogMessage(LMOFile, RhinoMessages);
	}
    }

    return 0;
}

int Rhino::ProcessStatusMsg(unsigned int _uiStatusCode)
/***************************************************************/
/*  Rutina de manejo de mensajes de estado del sistema         */
/***************************************************************/
/*  FUNCION PRIVADA                                            */
/***************************************************************/
{
     if((_uiStatusCode < 0) || (_uiStatusCode > (MAX_RC_STATUS_CODES + MAX_RB_STATUS_CODES)))
    {
	ProcessErrorMsg(ERR_CODE);
    }
    else
    {
	uiStatusCode = _uiStatusCode;

	get_StatusMsg(cStatusMessage);
	sprintf(RhinoMessages, "RhinoClass->ProcessStatusMsg:\n\tUltimo cmd enviado: %s\n\tUltima cadena recibida: %s\n\tCodigo de estado: %d\n\tLog Level: %d\n\tMSG: %s\n", LastCmdAndMsg.LastCommandtoRhino, LastCmdAndMsg.LastMessageFromRhino, uiStatusCode, uiLogLevel, cStatusMessage);

	if(uiLogLevel == 0)
	{
	    LogMessage(LMOCBk, RhinoMessages);
	}
	if(uiLogLevel == 1)
	{
	    LogMessage(LMODisplay, RhinoMessages);
	}
	if(uiLogLevel == 2)
	{
	    LogMessage(LMOFile, RhinoMessages);
	}
	if(uiLogLevel == 3)
	{
	    LogMessage(LMODisplay, RhinoMessages);
	    LogMessage(LMOFile, RhinoMessages);
	}
    }

    return 0;
}

int  Rhino::get_ErrorCode(unsigned int *_uiErrorCode)
/***************************************************************/
/*  Devuelve el codigo de error del ultimo error producido     */
/***************************************************************/
{
    *_uiErrorCode = uiErrorCode;
    return 0;
}

int  Rhino::DisplayAllErrorMessages(void)
/***************************************************************/
/*  Imprime todos los mensajes de error                        */
/***************************************************************/
{
    sprintf(RhinoMessages, "ERROR MESSEGES\n\n");
    LogMessage(LMODisplay, RhinoMessages);
    int i=0;
    
    sprintf(RhinoMessages, "RHINO CONTROLLER ERRORS\n");
    LogMessage(LMODisplay, RhinoMessages);
    for(i=0; i<MAX_RB_ERROR_CODES ; i++)
    {
	sprintf(RhinoMessages, "CODE:%d %s\n", i, RhinoBotErrorMsg[i]);
	LogMessage(LMODisplay, RhinoMessages);
    }

    sprintf(RhinoMessages, "\nRHINO CLASS ERRORS\n");
    LogMessage(LMODisplay, RhinoMessages);
    for(i=0; i<MAX_RC_ERROR_CODES ; i++)
    {
	sprintf(RhinoMessages, "CODE:%d %s\n", i, RhinoClassErrorMsg[i]);
	LogMessage(LMODisplay, RhinoMessages);
    }

    return 0;
}

int Rhino::LogMessage(int _LogType, char * _Message)
/***************************************************************/
/*  Muestra y almacena mensajes del sistema                    */
/***************************************************************/
{
    if(_LogType == LMOCBk)
    {
	strcpy(CallBack.sCBkMessage, _Message);
	CallBack.bNewMsg = true;
    }

    if(_LogType == LMODisplay)
    {
	fprintf(stderr, "%s", _Message);
    }

    if(_LogType == LMOFile)
    {
	pFile = fopen("rc_error_log.txt","a+");
	if(pFile == NULL){
	    ProcessErrorMsg(NO_FILE);
	    return -1;
	}
	fprintf(pFile,"%s\n", _Message);
	fclose(pFile);
    }

    return 0;
}

int Rhino::CBkMsg(void)
/***************************************************************/
/*  Si hay algun mensaje pendiente por reportar devuelve true  */
/***************************************************************/
{
    if(CallBack.bNewMsg == true)
	return 1;
    else 
	return 0;
}

int Rhino::get_CBkMsg(char * _Message)
/***************************************************************/
/* Devulve el ultimo mensaje reportado por la clase            */
/***************************************************************/
{
    if(CallBack.bNewMsg == true){
	strcpy(_Message, CallBack.sCBkMessage);
	CallBack.bNewMsg = false;
    }
    else
	strcpy(_Message, "No new message from Rhino Class\n");

    return 0;
}

int Rhino::InitMessageVectors(char _RhinoBotErrorMsg[MAX_RB_ERROR_CODES][MSG_LENGTH], char _RhinoClassErrorMsg[MAX_RC_ERROR_CODES][MSG_LENGTH], char _RhinoBotStatusMsg[MAX_RB_STATUS_CODES][MSG_LENGTH], char _RhinoClassStatusMsg[MAX_RC_STATUS_CODES][MSG_LENGTH])
/***************************************************************/
/* Inicializa los vectores pasados como parametro con con los  */ 
/* mensajes de error de la clase                               */
/***************************************************************/
/* ERROR CODE:  "unsigned int uiErrorCode"
                 MAX_RB_ERROR_CODES
                 MAX_RC_ERROR_CODES
		 MSG_LENGTH

   - de "0" a "MAX_RB_ERROR_CODES-1":
         Errores pertenecientes al controlador del brazo Rhino.

   - de "MAX_RB_ERROR_CODES" a "MAX_RB_ERROR_CODES + MAX_RC_ERROR_CODES": 
         Errores pertenecientes a la clase Rhino.

   -etc...

   - MSG_LENGTH es el largo maximo de los mensajes de error almacenados los 
     vectores
*/
{
    const char RhinoBotErrorMsgSOURCE[MAX_RB_ERROR_CODES][MSG_LENGTH]={
	{"	Rhino Controller Error:	BE HAPY, NO PROBLEM	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Invalid command.	"},
	{"	Rhino Controller Error:	Parameter out of bounds.	"},
	{"	Rhino Controller Error:	Missing parameter.	"},
	{"	Rhino Controller Error:	Expected delimiter not seen.	"},
	{"	Rhino Controller Error:	Command string too long.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	The teach pendant is active or busy.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Host input buffer overflow.	"},
	{"	Rhino Controller Error:	Host USART timed out.	"},
	{"	Rhino Controller Error:	Host USART error (framing, parity, etc.).	"},
	{"	Rhino Controller Error:	Teach pendant input buffer overflow.	"},
	{"	Rhino Controller Error:	Teach pendant USART timed out.	"},
	{"	Rhino Controller Error:	Teach pendant USART error (framing, parity, etc.).	"},
	{"	Rhino Controller Error:	Teach pendant USART overrun.	"},
	{"	Rhino Controller Error:	Host USART overrun.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Bad RAM location.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Teach pendant returned diagnostic error.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Teach pendant not present.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Missing label.	"},
	{"	Rhino Controller Error:	No program in memory.	"},
	{"	Rhino Controller Error:	Insufficient teach pendant memory.	"},
	{"	Rhino Controller Error:	Insufficient EEPROM memory.	"},
	{"	Rhino Controller Error:	Can't replace first record.	"},
	{"	Rhino Controller Error:	A pendant program already exists.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Hard home routine failed.	"},
	{"	Rhino Controller Error:	Hard home not set.	"},
	{"	Rhino Controller Error:	Soft home not set.	"},
	{"	Rhino Controller Error:	Arithmetic overflow.	"},
	{"	Rhino Controller Error:	Trig function returned error.	"},
	{"	Rhino Controller Error:	Error stack overflow.	"},
	{"	Rhino Controller Error:	Still executing a trapezoidal move.	"},
	{"	Rhino Controller Error:	Inactive motor referenced.	"},
	{"	Rhino Controller Error:	Insufficient move data (velocity or acceleration = 0).	"},
	{"	Rhino Controller Error:	Improper motor mode for command.	"},
	{"	Rhino Controller Error:	Limit switch A not found.	"},
	{"	Rhino Controller Error:	Limit switch B not found.	"},
	{"	Rhino Controller Error:	Limit switch C not found.	"},
	{"	Rhino Controller Error:	Limit switch D not found.	"},
	{"	Rhino Controller Error:	Limit switch E not found.	"},
	{"	Rhino Controller Error:	Limit switch F not found.	"},
	{"	Rhino Controller Error:	Limit switch G not found.	"},
	{"	Rhino Controller Error:	Limit switch H not found.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Interpolation move out of bounds.	"},
	{"	Rhino Controller Error:	xvz position out of bounds.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Limit switch A stuck.	"},
	{"	Rhino Controller Error:	Limit switch B stuck.	"},
	{"	Rhino Controller Error:	Limit switch C stuck.	"},
	{"	Rhino Controller Error:	Limit switch D stuck.	"},
	{"	Rhino Controller Error:	Lim it switch E stuck.	"},
	{"	Rhino Controller Error:	Limit switch F stuck.	"},
	{"	Rhino Controller Error:	Lim it switch G stuck.	"},
	{"	Rhino Controller Error:	Lim it switch H stuck. 	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Invalid robot type specifier.	"},
	{"	Rhino Controller Error:	Invalid motor specifier.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Invalid pendant mode.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Invalid xyz specifier. 	"},
	{"	Rhino Controller Error:	Invalid xyz parameter.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Motor A stalled.	"},
	{"	Rhino Controller Error:	Motor B stalled.	"},
	{"	Rhino Controller Error:	Motor C stalled.	"},
	{"	Rhino Controller Error:	Motor D stalled.	"},
	{"	Rhino Controller Error:	Motor E stalled.	"},
	{"	Rhino Controller Error:	Motor F stalled.	"},
	{"	Rhino Controller Error:	Motor G stalled.	"},
	{"	Rhino Controller Error:	Motor H stalled.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	Motor A's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Motor B's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Motor C's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Motor D's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Motor E's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Motor F's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Motor G's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Motor H's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Aux Port 1's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	Aux Port 2's current limit circuit was activated.	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"},
	{"	Rhino Controller Error:	NO ERROR	"}   
    };
    
    const char RhinoClassErrorMsgSOURCE[MAX_RC_ERROR_CODES][MSG_LENGTH]={
	{"	Rhino Class Error:	BE HAPY, NO PROBLEM	"},
	{"	Rhino Class Error:	'Error Code' fuera de rango	"},
	{"	Rhino Class Error:	'Log Level' fuera de rango	"},
	{"	Rhino Class Error:	No se pudo crear el archivo de registro "},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"},
	{"	Rhino Class Error:	NO ERROR	"}
    };

    for(int i=0; i<MAX_RB_ERROR_CODES; i++)
	strcpy(_RhinoBotErrorMsg[i], RhinoBotErrorMsgSOURCE[i]);

    for(int i=0; i<MAX_RC_ERROR_CODES; i++)
	strcpy(_RhinoClassErrorMsg[i], RhinoClassErrorMsgSOURCE[i]);

/*Falta completar los mensajes del sistemas*/
    const char RhinoBotStatusMsgSOURCE[MAX_RB_STATUS_CODES][MSG_LENGTH]={
	{"	Rhino Controller Status:	BE HAPY, NO PROBLEM	"},
	{"	Rhino Controller Status:	At least one motor is performing a trapezoidal move"},
	{"	Rhino Controller Status:	A system error has ocurred"},
	{"	Rhino Controller Status:	The general purpose delay timer is active	"},
	{"	Rhino Controller Status:	At least one wait on input or wait on switch is still pending	"},
	{"	Rhino Controller Status:	No teach pedant is connected	"},
	{"	Rhino Controller Status:	The teach pedant ENTER key hast been pressed	"},
	{"	Rhino Controller Status:	The teach pedant ESCAPER key hast been pressed	"},
	{"	Rhino Controller Status:	A teach pedant error has ocurred	"},	
	{"	Rhino Controller Status:	Motor A is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	Motor B is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	Motor C is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	Motor D is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	Motor E is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	Motor F is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	Motor G is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	Motor H is executing a trapezoidal move	"},
	{"	Rhino Controller Status:	The motor is either stationary or in a mode other than trapezoidal "},
	{"	Rhino Controller Status:	NC "}
    };
    const char RhinoClassStatusMsgSOURCE[MAX_RC_STATUS_CODES][MSG_LENGTH]={
	{"	Rhino Class Status:	BE HAPY, NO PROBLEM	"},
	{"	Rhino Class Status:	Se asume LogLevel 3 por defecto	"},
	{"	Rhino Class Status:	Depuracion de comando pendiente	"},
	{"	Rhino Class Status:	NC	"}
    };
    
    for(int i=0; i<MAX_RB_STATUS_CODES; i++)
	strcpy(_RhinoBotStatusMsg[i], RhinoBotStatusMsgSOURCE[i]);
    
    for(int i=0; i<MAX_RC_STATUS_CODES; i++)
	strcpy(_RhinoClassStatusMsg[i], RhinoClassStatusMsgSOURCE[i]);
    
    return 0;
}

int Rhino::ProcessCommandToRhino(char * _sCommand)
/***************************************************************/
/* Esta funcion procesa los comandos de estado del sistema     */
/* Parametros:                                                 */
/* [Status cmd I(char *)                                       */
/* I:in O:out                                                  */
/***************************************************************/
{
    if(!strcmp(_sCommand, "SA") || !strcmp(_sCommand, "sa"))
 	LastCmdAndMsg.uiLastCommandtoRhino = SA;
//     else if(!strcmp(_sCommand, "SC") || !strcmp(_sCommand, "sc"))
// 	LastCmdAndMsg.uiLastCommandtoRhino = SC;
    else if(!strcmp(_sCommand, "SE") || !strcmp(_sCommand, "se"))
 	LastCmdAndMsg.uiLastCommandtoRhino = SE;
//     else if(!strcmp(_sCommand, "SM") || !strcmp(_sCommand, "sm"))
// 	LastCmdAndMsg.uiLastCommandtoRhino = SM;
//     else if(!strcmp(_sCommand, "SP") || !strcmp(_sCommand, "sp"))
// 	LastCmdAndMsg.uiLastCommandtoRhino = SP;
    else if(!strcmp(_sCommand, "SS") || !strcmp(_sCommand, "ss"))
 	LastCmdAndMsg.uiLastCommandtoRhino = SS;
//     else if(!strcmp(_sCommand, "SU") || !strcmp(_sCommand, "su"))
// 	LastCmdAndMsg.uiLastCommandtoRhino = SU;
//     else if(!strcmp(_sCommand, "SV") || !strcmp(_sCommand, "sv"))
// 	LastCmdAndMsg.uiLastCommandtoRhino = SV;
//     else if(!strcmp(_sCommand, "SX") || !strcmp(_sCommand, "sx"))
// 	LastCmdAndMsg.uiLastCommandtoRhino = SX;
//     else if(!strcmp(_sCommand, "SZ") || !strcmp(_sCommand, "sz"))
// 	LastCmdAndMsg.uiLastCommandtoRhino = SZ;
    else
	LastCmdAndMsg.uiLastCommandtoRhino = NC;
   
    strcpy(LastCmdAndMsg.LastCommandtoRhino, _sCommand);
    strcpy(LastCmdAndMsg.LastMessageFromRhino, "");

    return 0;
/*CODIFICACION PENDIENTE*/
}

int Rhino::ProcessCommandToRhino(char * _sCommand, unsigned int *_iCmdCode)
/***************************************************************/
/* Esta funcion procesa los comandos de estado del sistema y   */
/* devuelve el enumerador correspondiente a ese comando        */  
/* Parametros:                                                 */
/* [Status cmd I(char *); Status cmd code O(unsigned int)]     */
/* I:in O:out                                                  */
/***************************************************************/
{
    if(!strcmp(_sCommand, "SA") || !strcmp(_sCommand, "sa")){
 	LastCmdAndMsg.uiLastCommandtoRhino = SA;
	*_iCmdCode = SA;
    }
//    else if(!strcmp(_sCommand, "SC") || !strcmp(_sCommand, "sc"))
//	*_iCmdCode = SC;
    else if(!strcmp(_sCommand, "SE") || !strcmp(_sCommand, "se")){
 	LastCmdAndMsg.uiLastCommandtoRhino = SE;
	*_iCmdCode = SE;
    }
//    else if(!strcmp(_sCommand, "SM") || !strcmp(_sCommand, "sm"))
//	*_iCmdCode = SM;
//    else if(!strcmp(_sCommand, "SP") || !strcmp(_sCommand, "sp"))
//	*_iCmdCode = SP;
    else if(!strcmp(_sCommand, "SS") || !strcmp(_sCommand, "ss")){
 	LastCmdAndMsg.uiLastCommandtoRhino = SS;
	*_iCmdCode = SS;
    }
//    else if(!strcmp(_sCommand, "SU") || !strcmp(_sCommand, "su"))
// 	*_iCmdCode = SU;
//     else if(!strcmp(_sCommand, "SV") || !strcmp(_sCommand, "sv"))
// 	*_iCmdCode = SV;
//     else if(!strcmp(_sCommand, "SX") || !strcmp(_sCommand, "sx"))
// 	*_iCmdCode = SX;
//     else if(!strcmp(_sCommand, "SZ") || !strcmp(_sCommand, "sz"))
// 	*_iCmdCode = SZ;
    else{
 	LastCmdAndMsg.uiLastCommandtoRhino = NC;
	*_iCmdCode = NC;
    }

    strcpy(LastCmdAndMsg.LastCommandtoRhino, _sCommand);
    strcpy(LastCmdAndMsg.LastMessageFromRhino, "");
    
    return 0;
/*CODIFICACION PENDIENTE*/
}

int Rhino::ProcessMessageFromRhino(char * _StringCode)
/***************************************************************/
/* Esta funcion procesa la respuesta del Rhino ante petitorios */
/* de estado y asocia los mensajes de error y estado           */
/* correspondientes.                                           */
/* Parametros:                                                 */
/* Rhino message I(char *);                                    */
/***************************************************************/
{
    strcpy(LastCmdAndMsg.LastMessageFromRhino, _StringCode);

    if(LastCmdAndMsg.uiLastCommandtoRhino != NC){
	
	unsigned int _uiCode = atoi(_StringCode);
	
	switch(LastCmdAndMsg.uiLastCommandtoRhino)
	{
	    case SS:
		uiStatusCode = _uiCode;
		
		if(_uiCode & 128){
		    ProcessStatusMsg(RB_MOTOR_MOVE);
		}
		if(_uiCode & 64){
		    ProcessStatusMsg(RB_ERROR);
		}
		if(_uiCode & 32){
		    ProcessStatusMsg(RB_DTIMER_ACT);
		}
		if(_uiCode & 16){
		    ProcessStatusMsg(RB_WAIT_PENDING);
		}
		if(_uiCode & 8){
		    ProcessStatusMsg(RB_NO_TP);
		}
		if(_uiCode & 4){
		    ProcessStatusMsg(RB_TP_ENTER);
		}
		if(_uiCode & 2){
		    ProcessStatusMsg(RB_TP_ESC);
		}
		if(_uiCode & 1){
		    ProcessStatusMsg(RB_TP_ERROR);
		}
		if(_uiCode == 0){
		    ProcessStatusMsg(RB_STS_NS);
		}
		break;
		
	    case SA:
		uiStatusCode = _uiCode;
		
		if(_uiCode & 128){
		    ProcessStatusMsg(RB_MOTOR_H_TMON);
		}
		if(_uiCode & 64){
		    ProcessStatusMsg(RB_MOTOR_G_TMON);
		}
		if(_uiCode & 32){
		    ProcessStatusMsg(RB_MOTOR_F_TMON);
		}
		if(_uiCode & 16){
		    ProcessStatusMsg(RB_MOTOR_E_TMON);
		}
		if(_uiCode & 8){
		    ProcessStatusMsg(RB_MOTOR_D_TMON);
		}
		if(_uiCode & 4){
		    ProcessStatusMsg(RB_MOTOR_C_TMON);
		}
		if(_uiCode & 2){
		    ProcessStatusMsg(RB_MOTOR_B_TMON);
		}
		if(_uiCode & 1){
		    ProcessStatusMsg(RB_MOTOR_A_TMON);
		}
		if(_uiCode == 0){
		    ProcessStatusMsg(RB_MOTORS_NOTM);
		}
		break;
		
	    case SE:
		uiErrorCode = _uiCode;
		ProcessErrorMsg(_uiCode);
		break;
		
	    default:
		ProcessStatusMsg(RC_DEP_CMD_PENDING);
		break;
	}
	return 0;   
    }
    
    ProcessStatusMsg(RC_DEP_CMD_PENDING);
    return 0;
/*CODIFICACION PENDIENTE*/
}

int Rhino::ProcessMessageFromRhino(unsigned int _iStatusCmd, char * _StringCode, unsigned int *_iCode, char * _sMessage)
/***************************************************************/
/* Esta funcion procesa la respuesta del Rhino ante petitorios */
/* de estado y asocia los mensajes de error y estado           */
/* orrespondientes.                                            */
/* Parametros:                                                 */
/* [Status comd code I(unsigned int); Rhino message I(char *); */ 
/* Rhino error code O(unsigned int);                           */
/* Rhino error mesage O(char *)]                               */
/***************************************************************/
{
    strcpy(LastCmdAndMsg.LastMessageFromRhino, _StringCode);

    if(LastCmdAndMsg.uiLastCommandtoRhino != NC){
	
	*_iCode = atoi(_StringCode);
	
	unsigned int uiCode = *_iCode;
	
	switch(_iStatusCmd)
	{
	    case SS:
		uiStatusCode = uiCode;
		
		if(uiCode & 128){
		    ProcessStatusMsg(RB_MOTOR_MOVE);
		    get_StatusMsg(_sMessage);  // :S
		}
		if(uiCode & 64){
		    ProcessStatusMsg(RB_ERROR);
		    get_StatusMsg(_sMessage);  // :(
		}
		if(uiCode & 32){
		    ProcessStatusMsg(RB_DTIMER_ACT);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 16){
		    ProcessStatusMsg(RB_WAIT_PENDING);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 8){
		    ProcessStatusMsg(RB_NO_TP);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 4){
		    ProcessStatusMsg(RB_TP_ENTER);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 2){
		    ProcessStatusMsg(RB_TP_ESC);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 1){
		    ProcessStatusMsg(RB_TP_ERROR);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode == 0){
		    ProcessStatusMsg(RB_STS_NS);
		    get_StatusMsg(_sMessage);  // :D
		}
		break;
		
	    case SE:
		uiErrorCode = uiCode;
		ProcessErrorMsg(uiCode);
		get_ErrorMsg(_sMessage);
		break;
		
	    case SA:
		uiStatusCode = uiCode;
		
		if(uiCode & 128){
		    ProcessStatusMsg(RB_MOTOR_H_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 64){
		    ProcessStatusMsg(RB_MOTOR_G_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 32){
		    ProcessStatusMsg(RB_MOTOR_F_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 16){
		    ProcessStatusMsg(RB_MOTOR_E_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 8){
		    ProcessStatusMsg(RB_MOTOR_D_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 4){
		    ProcessStatusMsg(RB_MOTOR_C_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 2){
		    ProcessStatusMsg(RB_MOTOR_B_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode & 1){
		    ProcessStatusMsg(RB_MOTOR_A_TMON);
		    get_StatusMsg(_sMessage);
		}
		if(uiCode == 0){
		    ProcessStatusMsg(RB_MOTORS_NOTM);
		    get_StatusMsg(_sMessage);
		}
		break;
		
	    default:
		ProcessStatusMsg(RC_DEP_CMD_PENDING);
		break;
	}
	return 0;    
    }
    
    ProcessStatusMsg(RC_DEP_CMD_PENDING);
    return 0;
/*CODIFICACION PENDIENTE*/
}

int Rhino::get_StatusMsg(char *_cTextStatus)
/***************************************************************/
/*  Devuelve le mensaje de estado del ultimo mensaje generado  */
/***************************************************************/
{
    /* Rhino Bot Status Messages */
    if(uiStatusCode >= 0 && uiStatusCode <= MAX_RB_STATUS_CODES)
    {
	strcpy(_cTextStatus, RhinoBotStatusMsg[uiStatusCode]);      
	return 0;
    }

    /* Rhino Class Status Messages */
    if(uiStatusCode > MAX_RB_STATUS_CODES && uiStatusCode <= (MAX_RB_STATUS_CODES + MAX_RC_STATUS_CODES))
    {
	strcpy(_cTextStatus, RhinoClassStatusMsg[uiStatusCode-MAX_RB_STATUS_CODES]);
	return 0;
    }
    
    ProcessErrorMsg(ERR_CODE);
    return -1;
}
int Rhino::get_StatusMsg(unsigned int _uiStatusCode, char *_cTextStatus)
/***************************************************************/
/*  Devuelve le mensaje de estado corrspondiente al codigo de  */
/*  status pasado como parametro                               */
/***************************************************************/
{
 /* Rhino Bot Status Messages*/
    if(_uiStatusCode >= 0 && _uiStatusCode <= MAX_RB_STATUS_CODES)
    {
	strcpy(_cTextStatus, RhinoBotStatusMsg[_uiStatusCode]);      
	return 0;
    }

    /* Rhino Class Status Messages*/
    if(_uiStatusCode > MAX_RB_STATUS_CODES && _uiStatusCode <= (MAX_RB_STATUS_CODES + MAX_RC_STATUS_CODES))
    {
	strcpy(_cTextStatus, RhinoClassStatusMsg[_uiStatusCode-MAX_RB_STATUS_CODES]);
	return 0;
    }
    
    ProcessErrorMsg(ERR_CODE);
    return -1;
}
int Rhino::get_StatusCode(unsigned int *_uiStatusCode)
/***************************************************************/
/*  Devuelve el codigo de status del ultimo mensaje generado   */
/***************************************************************/
{
    *_uiStatusCode = uiStatusCode;
    return 0;
}