 /*      
 *          Copyright (C) 2001 RasSimTech Ltd. 
 *
 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or (at your option) any later version. 
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details. 
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 
 */


//   $Revision: 1.0.1 $  $Date: 2001/03/31 00:00:00 $

#define S_FUNCTION_NAME NetworkReceiveAVDSBlock
#define S_FUNCTION_LEVEL 2


#include <afxwin.h>         // MFC core and standard components
//#include <afxmt.h>
#include "simstruc.h"
//#include "mex.h"
#include <map>

#include "AVDS2MATLAB.h"



enum enCallType {enOpenConnection=1,enSendData,enReceiveData,enCloseConnection}; 
	// using enCloseConnection as the last mode, if more modes are added insert them before enCloseConnection

#define SAMPLE_TIME 0.0


////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////   PARAMETER DEFINITIONS      //////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

#define ID_TO_AVDS_IDX 0
#define PARAM_ID_TO_AVDS(S) ssGetSFcnParam(S,ID_TO_AVDS_IDX)
 
#define NUMBER_SIGNALS_TO_AVDS_IDX 1
#define PARAM_NUMBER_SIGNALS_TO_AVDS(S) ssGetSFcnParam(S,NUMBER_SIGNALS_TO_AVDS_IDX)
 
#define ID_TO_SIMULINK_IDX 2
#define PARAM_ID_TO_SIMULINK(S) ssGetSFcnParam(S,ID_TO_SIMULINK_IDX)
 
#define NUMBER_SIGNALS_TO_SIMULINK_IDX 3
#define PARAM_NUMBER_SIGNALS_TO_SIMULINK(S) ssGetSFcnParam(S,NUMBER_SIGNALS_TO_SIMULINK_IDX)
 
#define NPARAMS 4


////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////   INPUT/OUTPUT DEFINITIONS   //////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
/*

INPUTS:
	- Number of vehicles, targets and tasks, i.e  8 vehicles, 10 targets, 2 tasks => 3 elements
	- Bennefit of continuing to search for each vehicle, i.e 8 vehicles => 8 elements
	- Bennefit of assigning each vehicle to target/task type, i.e 8 vehicles * 10 targets * 2 task types => 160 elements

																		178 total elements

OUTPUTS
	-Vector for each vehicle to target assignment, i.e. 8 vehicles = 8 elements
	-Vector for each vehicle to task type assignment, i.e. 8 vehicles = 8 elements
	-Bennefit Metric = 1 element
																	17 total elements

NOTE: the actual number and definitions of the inputs/outputs used will depend on the values for the 
      number of vehicles, targets and tasks.

*/

#define U(element) (*uPtrs[element])  /* Pointer to Input Port0 */

enum enInputs {	
				in,
				inTotalInputs };

enum enOutputs {
			out,
			outTotalOutputs };


extern "C"
{
	void mdlStart(SimStruct *S);
	void mdlInitializeSizes(SimStruct *S);
	void mdlInitializeSampleTimes(SimStruct *S);
	void mdlInitializeConditions(SimStruct *S);
	void mdlOutputs(SimStruct *S, int_T tid);
	void mdlUpdate(SimStruct *S, int_T tid);
	void mdlTerminate(SimStruct *S);
	void mdlCheckParameters(SimStruct *S);
}


/*====================*
 * S-function methods *
 *====================*/

#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
	/* Function: mdlCheckParameters =============================================
	 * Abstract:
	 *    Validate our parameters to verify they are okay.
	*/
	static void mdlCheckParameters(SimStruct *S)
	{
      /* Check 1st parameter: ID of the To AVDS connection */
      {
          if (!mxIsDouble(PARAM_ID_TO_AVDS(S)) || mxGetNumberOfElements(PARAM_ID_TO_AVDS(S)) != 1) 
		  {
              ssSetErrorStatus(S,"1st parameter to the SharedAVDSBlock S-function must be the "
                               "ID of the To AVDS connection");
              return;
          }
      }
 
      /* Check 2nd parameter: number of signals passed to AVDS */
      {
          if (!mxIsDouble(PARAM_NUMBER_SIGNALS_TO_AVDS(S)) || mxGetNumberOfElements(PARAM_NUMBER_SIGNALS_TO_AVDS(S)) != 1) 
		  {
              ssSetErrorStatus(S,"2nd parameter to the SharedAVDSBlock S-function must be the "
                               "number of signals passed to AVDS");
              return;
          }
      }

      /* Check 3rd parameter: ID of the To Simulink connection */
      {
          if (!mxIsDouble(PARAM_ID_TO_SIMULINK(S)) || mxGetNumberOfElements(PARAM_ID_TO_SIMULINK(S)) != 1) 
		  {
              ssSetErrorStatus(S,"3rd parameter to the SharedAVDSBlock S-function must be the "
                               "ID of the To Simulink connection");
              return;
          }
      }
 
      /* Check 4th parameter: number of signals passed to Simulink */
      {
          if (!mxIsDouble(PARAM_NUMBER_SIGNALS_TO_SIMULINK(S)) || mxGetNumberOfElements(PARAM_NUMBER_SIGNALS_TO_SIMULINK(S)) != 1) 
		  {
              ssSetErrorStatus(S,"4th parameter to the SharedAVDSBlock S-function must be the "
                               "number of signals passed to Simulink");
              return;
          }
      }
	}
#endif /* MDL_CHECK_PARAMETERS */


 
/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NPARAMS);  /* Number of expected parameters */

	ssSetNumContStates(S, 0);	// number continuous
    ssSetNumDiscStates(S, 0);	// number discrete states

    if (!ssSetNumInputPorts(S, 1)) 
	{
		return;
	}

	int iNumberInputs = *mxGetPr(PARAM_NUMBER_SIGNALS_TO_AVDS(S));
    ssSetInputPortWidth(S, 0, iNumberInputs);
    ssSetInputPortDirectFeedThrough(S, 0, 0);	//optional

    if (!ssSetNumOutputPorts(S, 1))
	{
		return;
	}
	int iNumberOutputs = *mxGetPr(PARAM_NUMBER_SIGNALS_TO_SIMULINK(S));
    ssSetOutputPortWidth(S, 0, iNumberOutputs);


    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Take care when specifying exception free code - see sfuntmpl.doc */
    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}


#define MDL_INITIALIZE_CONDITIONS
/* Function: mdlInitializeConditions ========================================
 * Abstract:
 *    Initialize both continuous states to zero.
 */
static void mdlInitializeConditions(SimStruct *S)
{
}

/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    Specifiy that we inherit our sample time from the driving block.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}

#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  /* Function: mdlStart =======================================================
   * Abstract:
   *    This function is called once at start of model execution. If you
   *    have states that should be initialized once, this is the place
   *    to do it.
   */
  static void mdlStart(SimStruct *S)
  {
	CAVDS2MATLAB* pAVDS2MATLAB = new CAVDS2MATLAB();
	if(!pAVDS2MATLAB)
	{
		ssSetErrorStatus(S,"SharedAVDSBlock: Error instantiating CAVDS2MATLAB class.");
		return;
	}
	ssSetUserData(S,(void*)pAVDS2MATLAB);

	// get AVDS ID
	int iIDAVDS = *mxGetPr(PARAM_ID_TO_AVDS(S));

	// get the Size of ToAVDS vector
	int iSizeToAVDS = *mxGetPr(PARAM_NUMBER_SIGNALS_TO_AVDS(S));

	// get MATLAB ID
	int iIDMATLAB = *mxGetPr(PARAM_ID_TO_SIMULINK(S));

	// get Size of ToMATLAB vector
	int iSizeToMATLAB = *mxGetPr(PARAM_NUMBER_SIGNALS_TO_SIMULINK(S));

	// open connection
	if(!pAVDS2MATLAB->bOpenConnection(iIDAVDS,iSizeToAVDS,iIDMATLAB,iSizeToMATLAB))
	{
		char caErrorString[512];
		sprintf(caErrorString,"SharedAVDSBlock:: Error encountered while opening the connection.\nArguments were:\niIDAVDS = %g,iSizeToAVDS = %g,iIDMATLAB = %g,iSizeToMATLAB = %g \n",iIDAVDS,iSizeToAVDS,iIDMATLAB,iSizeToMATLAB);
		ssSetErrorStatus(S,caErrorString);
		return;
	}

 		case enInitializeVehicle:	
			//NetworkAVDS(enInitializeVehicle,iIDConnection,iCraftType,dInitLongitude,dInitLatitude,dInitAltitude);
			{

				// first: check and get arguments
				// get iCraftType
				iIndexArg = 2;

				//  create a pointer to the input data vector 
				double* pdInitData = mxGetPr(prhs[iIndexArg]);
				//  get the dimensions of the input data vector 
				int iMrowsIn = mxGetM(prhs[iIndexArg]);
				int iNcolsIn = mxGetN(prhs[iIndexArg]);
				// check the dimensions 
				if(!((iMrowsIn==1) || (iNcolsIn==1)))	// must be column or row vector 
				{
					sprintf(caErrorString,"NetworkAVDS:: Input must be a vector\n");
					mexErrMsgTxt(caErrorString);
				}
				int iInputLength = iMrowsIn*iNcolsIn;
				if((iInputLength > enSizeInitData)||(iInputLength <= 0))	// must have the correct dimension
				{
					sprintf(caErrorString,"NetworkAVDS:: Wrong length of input vector.\nLength of input vector must be in the range [1,%d].\nThis input vector's length is %d\n",enSizeInitData,iInputLength);
					mexErrMsgTxt(caErrorString);
				}

				// default intial values:
				int iInputNum = enCraftType;
				int iCraftType =  (iInputLength>iInputNum)?(pdInitData[iInputNum]):(0);
				iInputNum = enInitLongitudeDegDecimal;
				double dInitLongitude = (iInputLength>iInputNum)?(pdInitData[iInputNum]):(-122.063400);
				iInputNum = enInitLatitudeDegDecimal;
				double dInitLatitude =  (iInputLength>iInputNum)?(pdInitData[iInputNum]):(37.981523);
				iInputNum = enInitAltitudeFeet;
				double dInitAltitude =  (iInputLength>iInputNum)?(pdInitData[iInputNum]):(5000.0);

				itMATLAB2Network = mpmatlab2Network.find(iIDConnection);	
				if(itMATLAB2Network != mpmatlab2Network.end())
				{
					mpmatlab2Network.erase(itMATLAB2Network);
				}
				CMATLAB2Network mpmatlab2NetworkTemp;
				std::pair<MCMATLAB2Network::iterator,bool> prMATLAB2Network = mpmatlab2Network.insert(MCMATLAB2Network::value_type(iIDConnection,mpmatlab2NetworkTemp));
				if(!prMATLAB2Network.second)
				{
					sprintf(caErrorString,"NetworkAVDS:: Error encountered while opening the connection.\n");
					mexErrMsgTxt(caErrorString);
				}
				itMATLAB2Network = prMATLAB2Network.first;
				CMATLAB2Network* pmatlab2Network = &(*itMATLAB2Network).second;
				// third: open connection
				if(!pmatlab2Network->bInitializeVehicle(iCraftType,dInitLongitude,dInitLatitude,dInitAltitude))
				{
					sprintf(caErrorString,"NetworkAVDS:: Error encountered while opening the connection.\n");
					mexErrMsgTxt(caErrorString);
				}
			}	//case enInitializeVehicle:

  
  }
#endif /*  MDL_START */


/* Function: mdlTerminate =====================================================
 * Abstract:
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(SimStruct *S)
{
	CAVDS2MATLAB* pAVDS2MATLAB = (CAVDS2MATLAB*)ssGetUserData(S);
	pAVDS2MATLAB->CloseConnection();
	delete 	pAVDS2MATLAB;


		case enCloseConnection:	//NetworkAVDS();
			// NetworkAVDS(enCloseConnection);
			{
				if(!mpmatlab2Network.empty())
				{
					itMATLAB2Network = mpmatlab2Network.begin();
					CMATLAB2Network* pmatlab2Network = &(*itMATLAB2Network).second;
					pmatlab2Network->CloseConnection();
					mpmatlab2Network.clear();
				}
				else
				{
					sprintf(caErrorString,"NetworkAVDS:: Error encountered while closing connection:\nno connection exists.\n");
					mexErrMsgTxt(caErrorString);
				}
			}
			break;
}



/* Function: mdlOutputs =======================================================
 * Abstract:
 *      y = Cx + Du 
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
	real_T* prealOutputs = ssGetOutputPortRealSignal(S,0);
	InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
	CAVDS2MATLAB* pAVDS2MATLAB = (CAVDS2MATLAB*)ssGetUserData(S);

	// check on the connection
	if(!pAVDS2MATLAB->bGetConnectionOpen())
	{
		ssSetErrorStatus(S,"SharedAVDSBlock:: Connection not open.");
		return;
	}

	// send data
	if(!pAVDS2MATLAB->bSendData((const double *const)uPtrs[0]))
	{
		ssSetErrorStatus(S,"SharedAVDSBlock:: Error encountered while sending data.");
		return;
	}

	// receive data
	if(!pAVDS2MATLAB->bReceiveData(prealOutputs))
	{
		ssSetErrorStatus(S,"SharedAVDSBlock:: Error encountered while receiving data.");
		return;
	}

		case enSendData:	//NetworkAVDS();
			//NetworkAVDS(enSendData,iIDConnection,pdSendData);
			{
				itMATLAB2Network = mpmatlab2Network.find(iIDConnection);	
				if(itMATLAB2Network == mpmatlab2Network.end())
				{
					sprintf(caErrorString,"NetworkAVDS:: Unable to   find connection #%d",iIDConnection);
					mexErrMsgTxt(caErrorString);
				}
				CMATLAB2Network* pmatlab2Network = &(*itMATLAB2Network).second;
				
				iIndexArg = 2;
				//  create a pointer to the input data vector 
				double* pdSendData = mxGetPr(prhs[iIndexArg]);
				//  get the dimensions of the input data vector 
				int iMrowsIn = mxGetM(prhs[iIndexArg]);
				int iNcolsIn = mxGetN(prhs[iIndexArg]);
				// check the dimensions 
				if(!((iMrowsIn==1) || (iNcolsIn==1)))	// must be column or row vector 
				{
					sprintf(caErrorString,"NetworkAVDS:: Input must be a vector\n");
					mexErrMsgTxt(caErrorString);
				}
				int iInputLength = iMrowsIn*iNcolsIn;
				if((iInputLength > enSizeSendData)||(iInputLength <= 0))	// must have the correct dimension
				{
					sprintf(caErrorString,"NetworkAVDS:: Wrong length of input vector.\nLength of input vector must be in the range [1,%d].\nThis input vector's length is %d\n",enSizeSendData,iInputLength);
					mexErrMsgTxt(caErrorString);
				}
				int iInputNum = en_pos_X;
				pmatlab2Network->m_acftVehicleSend.pos_X = pdSendData[iInputNum];
				iInputNum = en_pos_Y;
				pmatlab2Network->m_acftVehicleSend.pos_Y = (iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_pos_Z;
				pmatlab2Network->m_acftVehicleSend.pos_Z = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_X;
				pmatlab2Network->m_acftVehicleSend.rot_X = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_Y;
				pmatlab2Network->m_acftVehicleSend.rot_Y = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_Z;
				pmatlab2Network->m_acftVehicleSend.rot_Z = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_elev;
				pmatlab2Network->m_acftVehicleSend.elev = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_ail;
				pmatlab2Network->m_acftVehicleSend.ail = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rud;
				pmatlab2Network->m_acftVehicleSend.rud = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_engine;
				pmatlab2Network->m_acftVehicleSend.throttle = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				pmatlab2Network->m_acftVehicleSend.engine = (unsigned char)pmatlab2Network->m_acftVehicleSend.throttle;
				iInputNum = en_pos_dX;
				pmatlab2Network->m_acftVehicleSend.pos_dX = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_pos_dY;
				pmatlab2Network->m_acftVehicleSend.pos_dY = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_pos_dZ;
				pmatlab2Network->m_acftVehicleSend.pos_dZ = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_dX;
				pmatlab2Network->m_acftVehicleSend.rot_dX = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_dY;
				pmatlab2Network->m_acftVehicleSend.rot_dY = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_dZ;
				pmatlab2Network->m_acftVehicleSend.rot_dZ = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				//pmatlab2Network->m_acftVehicleSend.u = ;
				//pmatlab2Network->m_acftVehicleSend.v = ;
				//pmatlab2Network->m_acftVehicleSend.w = ;
				//pmatlab2Network->m_acftVehicleSend.p = ;
				//pmatlab2Network->m_acftVehicleSend.q = ;
				//pmatlab2Network->m_acftVehicleSend.r = ;
				pmatlab2Network->m_acftVehicleSend.dt = (float)pmatlab2Network->dGetDT();
				// call the function
				if(!pmatlab2Network->bSendData())
				{
					sprintf(caErrorString,"NetworkAVDS:: Error encountered while sending data.\n");
					mexErrMsgTxt(caErrorString);
				}
			}	//case enSendData:
			break;

}



#define MDL_UPDATE
/* Function: mdlUpdate ======================================================
 * Abstract:
 *      xdot = Ax + Bu
 */
static void mdlUpdate(SimStruct *S, int_T tid)
{
}



#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif


