 /*      
 *          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 AVDSNetworkReceiveBlock
#define S_FUNCTION_LEVEL 2


#include <afxwin.h>         // MFC core and standard components

#include "simstruc.h"

#include "MATLAB2Network.h"


#define SAMPLE_TIME 0.0


// intialize static variables
BOOL CMATLAB2Network::m_bConnectionOpen = FALSE;
std::list<Aircraft> CMATLAB2Network::m_lacftVehiclesReceive;
int CMATLAB2Network::m_iNumberVehicles = 0;

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

enum enParameters {
					paramTotalParameters};


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

enum enInputs {	
				inDummyInput,
				inTotalInputs };

enum enOutputs { 
		outElapsedTime,
		out_state,
		out_elev,
		out_ail,
		out_rud,
		out_throttle,
		out_u,
		out_v,
		out_w,
		out_p,
		out_q,
		out_r,
		out_ref_X,
		out_ref_Y,
		out_pos_X,
		out_pos_Y,
		out_pos_Z,
		out_pos_dX,
		out_pos_dY,
		out_pos_dZ,
		out_rot_X,
		out_rot_Y,
		out_rot_Z,
		out_rot_dX,
		out_rot_dY,
		out_rot_dZ,
		out_engine,
		out_hostid,
		//out_packet,
		out_dt,
		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)
	{
	}
#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, paramTotalParameters);  /* Number of expected parameters */

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

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

    ssSetInputPortWidth(S, 0, 1);
    ssSetInputPortDirectFeedThrough(S, 0, 1);	//not optional in R12

    if (!ssSetNumOutputPorts(S, 1))
	{
		return;
	}
    ssSetOutputPortWidth(S, 0, outTotalOutputs);


    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)
	{
		real_T* prealOutputs = ssGetOutputPortRealSignal(S,0);
		CMATLAB2Network* pMATLAB2Network = new CMATLAB2Network();
		if(!pMATLAB2Network)
		{
			ssSetErrorStatus(S,"AVDSNetworkReceiveBlock: Error instantiating CMATLAB2Network class.");
			return;
		}
		ssSetUserData(S,(void*)pMATLAB2Network);

		// open connection
		if(!pMATLAB2Network->bOpenConnection())
		{
			ssSetErrorStatus(S,"AVDSNetworkReceiveBlock: Error encountered while opening the connection..");
			return;
		}

	}
#endif /*  MDL_START */


/* Function: mdlTerminate =====================================================
 * Abstract:
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(SimStruct *S)
{
	CMATLAB2Network* pMATLAB2Network = (CMATLAB2Network*)ssGetUserData(S);
	pMATLAB2Network->CloseConnection();	//this closes all open connections
	delete 	pMATLAB2Network;
}



/* 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);
	CMATLAB2Network* pMATLAB2Network = (CMATLAB2Network*)ssGetUserData(S);


	// call the receive function to keep receiving packets
	pMATLAB2Network->bReceiveData();

	//collect one of the packets that have been received
	//TODO:: could collect all of the waiting packets
	if (!pMATLAB2Network->m_lacftVehiclesReceive.empty())
	{
		Aircraft* pacftTemp = &(*pMATLAB2Network->m_lacftVehiclesReceive.begin());
		prealOutputs[out_state] = pacftTemp->state;
		prealOutputs[out_elev] = pacftTemp->elev;
		prealOutputs[out_ail] = pacftTemp->ail;
		prealOutputs[out_rud] = pacftTemp->rud;
		prealOutputs[out_throttle] = pacftTemp->throttle;
		prealOutputs[out_u] = pacftTemp->u;
		prealOutputs[out_v] = pacftTemp->v;
		prealOutputs[out_w] = pacftTemp->w;
		prealOutputs[out_p] = pacftTemp->p;
		prealOutputs[out_q] = pacftTemp->q;
		prealOutputs[out_r] = pacftTemp->r;
		prealOutputs[out_ref_X] = pacftTemp->ref_X;
		prealOutputs[out_ref_Y] = pacftTemp->ref_Y;
		prealOutputs[out_pos_X] = pacftTemp->pos_X;
		prealOutputs[out_pos_Y] = pacftTemp->pos_Y;
		prealOutputs[out_pos_Z] = pacftTemp->pos_Z;
		prealOutputs[out_pos_dX] = pacftTemp->pos_dX;
		prealOutputs[out_pos_dY] = pacftTemp->pos_dY;
		prealOutputs[out_pos_dZ] = pacftTemp->pos_dZ;
		prealOutputs[out_rot_X] = pacftTemp->rot_X;
		prealOutputs[out_rot_Y] = pacftTemp->rot_Y;
		prealOutputs[out_rot_Z] = pacftTemp->rot_Z;
		prealOutputs[out_rot_dX] = pacftTemp->rot_dX;
		prealOutputs[out_rot_dY] = pacftTemp->rot_dY;
		prealOutputs[out_rot_dZ] = pacftTemp->rot_dZ;
		prealOutputs[out_engine] = pacftTemp->engine;
		prealOutputs[out_hostid] = pacftTemp->hostid;
		//prealOutputs[out_packet] = pacftTemp->packet;
		prealOutputs[out_dt] = pacftTemp->dt;
	}		//if (!pMATLAB2Network->m_lacftVehiclesReceive.empty())
	prealOutputs[outElapsedTime] = pMATLAB2Network->dGetSimulationTime();

}



#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


