 /*      
 *          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 AVDSSimulationBlock
#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"



//typedef std::map<int,CAVDS2MATLAB> MCAVDS2MATLAB;
//static MCAVDS2MATLAB mpavds2Matlab;

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      //////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
enum enParameters
{
	paramIDToAVDS,
	paramNumberSignalsToAVDS,
	paramIDToSimulink,
	paramNumberSignalsToSimulink,
	paramRealTimeFlag,
	paramTotal,
};


#define PARAM_ID_TO_AVDS(S) ssGetSFcnParam(S,paramIDToAVDS)
 
#define PARAM_NUMBER_SIGNALS_TO_AVDS(S) ssGetSFcnParam(S,paramNumberSignalsToAVDS)
 
#define PARAM_ID_TO_SIMULINK(S) ssGetSFcnParam(S,paramIDToSimulink)
 
#define PARAM_NUMBER_SIGNALS_TO_SIMULINK(S) ssGetSFcnParam(S,paramNumberSignalsToSimulink)
 
#define PARAM_NUMBER_USE_REAL_TIME_FLAG(S) ssGetSFcnParam(S,paramRealTimeFlag)
 

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////   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 {	
				inAVDSTime,
				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 AVDSSimulationBlock 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 AVDSSimulationBlock 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 AVDSSimulationBlock 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 AVDSSimulationBlock S-function must be the "
                               "number of signals passed to Simulink");
              return;
          }
      }
 
      /* Check 5th parameter: use AVDS Time Flag */
      {
          if (!mxIsDouble(PARAM_NUMBER_USE_REAL_TIME_FLAG(S)) || mxGetNumberOfElements(PARAM_NUMBER_USE_REAL_TIME_FLAG(S)) != 1) 
		  {
              ssSetErrorStatus(S,"5th parameter to the AVDSSimulationBlock S-function must be the "
                               "Use AVDS Time Flag, i.e. (0 or 1)");
              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, paramTotal);  /* 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, 1);	//not optional in R12

    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,"AVDSSimulationBlock: 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));

	// get use Real time flag
	pAVDS2MATLAB->bGetUseRealTimeFlag() = static_cast<BOOL>(*mxGetPr(PARAM_NUMBER_USE_REAL_TIME_FLAG(S)));

	if(pAVDS2MATLAB->bGetUseRealTimeFlag())
	{
		pAVDS2MATLAB->iStartElapsedTimer();
	}

	// open connection
	if(!pAVDS2MATLAB->bOpenConnection(iIDAVDS,iSizeToAVDS,iIDMATLAB,iSizeToMATLAB))
	{
		char caErrorString[512];
		sprintf(caErrorString,"AVDSSimulationBlock:: 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;
	}

  }
#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;
}



/* 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,"AVDSSimulationBlock:: Connection not open.");
		return;
	}

	BOOL bLoop = pAVDS2MATLAB->bGetUseRealTimeFlag();
	do
	{
		double dTimeReal = pAVDS2MATLAB->dGetTimeSinceStartSec();
		double dTimeSimulink = ssGetT(S);
		bLoop = ((bLoop)&&(dTimeReal<dTimeSimulink));
	}
	while(bLoop);

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

}



#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


