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


#include <afxwin.h>         // MFC core and standard components
#include <stdlib.h>	// rand()
#include <stdio.h>	//snprintf
#include <time.h>	//time()

#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 
{
	paramVehicleHandle,
	paramTransmitRateHz,
	paramCraftType,
	paramInitLatitudeDeg,
	paramInitLongitudeDeg,
	paramInitAltitudeFeet,
	paramRealTimeFlag,
	paramTotalParameters
};

#define PARAM_VEHICLE_HANDLE(S) ssGetSFcnParam(S,paramVehicleHandle)
#define PARAM_TRANSMIT_RATE_HZ(S) ssGetSFcnParam(S,paramTransmitRateHz)
#define PARAM_CRAFT_TYPE(S) ssGetSFcnParam(S,paramCraftType)
#define PARAM_INIT_LATITUDE(S) ssGetSFcnParam(S,paramInitLatitudeDeg)
#define PARAM_INIT_LONGITUDE(S) ssGetSFcnParam(S,paramInitLongitudeDeg)
#define PARAM_INIT_ALTITUDE(S) ssGetSFcnParam(S,paramInitAltitudeFeet)
#define PARAM_NUMBER_USE_REAL_TIME_FLAG(S) ssGetSFcnParam(S,paramRealTimeFlag)
 



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

enum enInputs {	
				in_pos_X,in_pos_Y,in_pos_Z,in_rot_X,in_rot_Y,in_rot_Z,
				in_elev,in_ail,in_rud,in_engine,
				inTotalInputs };

enum enOutputs { 
				outElapsedTime,
				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: Vehicle Handle */
      {
          if (!mxIsChar(PARAM_VEHICLE_HANDLE(S)) != 1) 
		  {
              ssSetErrorStatus(S,"1st parameter to the AVDSNetworkSendBlock must be the "
                               "Vehicle Handle name.");
              return;
          }
      }
 
      /* Check 2nd parameter: Transmit Rate in Hz */
      {
          if (!mxIsDouble(PARAM_TRANSMIT_RATE_HZ(S)) || mxGetNumberOfElements(PARAM_TRANSMIT_RATE_HZ(S)) != 1) 
		  {
              ssSetErrorStatus(S,"2nd parameter to the AVDSNetworkSendBlock must be the "
                               "Transmit Rate in Hz");
              return;
          }
      }
 
      /* Check 3rd parameter: Craft Type */
      {
          if (!mxIsDouble(PARAM_CRAFT_TYPE(S)) || mxGetNumberOfElements(PARAM_CRAFT_TYPE(S)) != 1) 
		  {
              ssSetErrorStatus(S,"3rd parameter to the AVDSNetworkSendBlock must be the "
                               "Craft Type");
              return;
          }
      }
 
      /* Check 4th parameter: Initial Longitude (degrees decimal) */
      {
          if (!mxIsDouble(PARAM_INIT_LONGITUDE(S)) || mxGetNumberOfElements(PARAM_INIT_LONGITUDE(S)) != 1) 
		  {
              ssSetErrorStatus(S,"4th parameter to the AVDSNetworkSendBlock must be the "
                               "Initial Longitude (degrees decimal)");
              return;
          }
      }

      /* Check 5th parameter: Initial Latitude (degrees decimal) */
      {
          if (!mxIsDouble(PARAM_INIT_LATITUDE(S)) || mxGetNumberOfElements(PARAM_INIT_LATITUDE(S)) != 1) 
		  {
              ssSetErrorStatus(S,"5th parameter to the AVDSNetworkSendBlock must be the "
                               "Initial Latitude (degrees decimal)");
              return;
          }
      }
 
      /* Check 6th parameter: Initial Alitude (feet) */
      {
          if (!mxIsDouble(PARAM_INIT_ALTITUDE(S)) || mxGetNumberOfElements(PARAM_INIT_ALTITUDE(S)) != 1) 
		  {
              ssSetErrorStatus(S,"6th parameter to the AVDSNetworkSendBlock must be the "
                               "Initial Alitude (feet)");
              return;
          }
      }
 
      /* Check 7th 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,"7th parameter to the AVDSNetworkSendBlock 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, paramTotalParameters);  /* Number of expected parameters */

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

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

    ssSetInputPortWidth(S, 0, inTotalInputs);
    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,"AVDSNetworkSendBlock: Error instantiating CMATLAB2Network class.");
			return;
		}
		ssSetUserData(S,(void*)pMATLAB2Network);

		// intial values:
		char caVehicleHandle[PACKET_HANDLE_SIZE];
		char caVehicleHandleSmall[PACKET_HANDLE_SIZE-4];
		mxGetString(PARAM_VEHICLE_HANDLE(S),caVehicleHandleSmall,PACKET_HANDLE_SIZE-4);
		srand((unsigned)time(NULL));	// seed random number with time
		int iRandomNumber = rand();
		iRandomNumber &= 0x270F; // only want last four digits
		_snprintf(caVehicleHandle,PACKET_HANDLE_SIZE,"%s%d",caVehicleHandleSmall,iRandomNumber);

		int iInputCraftType = *mxGetPr(PARAM_CRAFT_TYPE(S));
		int iCraftType =  (iInputCraftType>0)?(iInputCraftType):(0);
		double dInitLongitude = *mxGetPr(PARAM_INIT_LONGITUDE(S));
		double dInitLatitude =  *mxGetPr(PARAM_INIT_LATITUDE(S));
		double dInitAltitude =  *mxGetPr(PARAM_INIT_ALTITUDE(S));

		// open connection
		if(!pMATLAB2Network->bInitializeVehicle(iCraftType,dInitLongitude,dInitLatitude,dInitAltitude,caVehicleHandle))
		{
			ssSetErrorStatus(S,"AVDSNetworkSendBlock:: Error encountered while opening the connection.\n");
			return;
		}

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

		pMATLAB2Network->dGetDT();	// start elapsed timer
		pMATLAB2Network->m_acftVehicleSend.dt = 0.0;
		prealOutputs[outElapsedTime] = 0.0;

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

	// X and Y axes are swaped!!!
	pMATLAB2Network->m_acftVehicleSend.pos_X = U(in_pos_Y);
	pMATLAB2Network->m_acftVehicleSend.pos_Y = U(in_pos_X);
	pMATLAB2Network->m_acftVehicleSend.pos_Z = U(in_pos_Z);
	pMATLAB2Network->m_acftVehicleSend.rot_X = U(in_rot_Y);
	pMATLAB2Network->m_acftVehicleSend.rot_Y = U(in_rot_X);
	pMATLAB2Network->m_acftVehicleSend.rot_Z = U(in_rot_Z);
	pMATLAB2Network->m_acftVehicleSend.elev = U(in_elev);
	pMATLAB2Network->m_acftVehicleSend.ail = U(in_ail);
	pMATLAB2Network->m_acftVehicleSend.rud = U(in_rud);
	pMATLAB2Network->m_acftVehicleSend.throttle = U(in_engine);
	pMATLAB2Network->m_acftVehicleSend.engine = U(in_engine);
#ifdef notdef
	pMATLAB2Network->m_acftVehicleSend.pos_dX = U(in_pos_dY);
	pMATLAB2Network->m_acftVehicleSend.pos_dY = U(in_pos_dX);
	pMATLAB2Network->m_acftVehicleSend.pos_dZ = U(in_pos_dZ);
	pMATLAB2Network->m_acftVehicleSend.rot_dX = U(in_rot_dY);
	pMATLAB2Network->m_acftVehicleSend.rot_dY = U(in_rot_dX);
	pMATLAB2Network->m_acftVehicleSend.rot_dZ = U(in_rot_dZ);
#else	//#ifdef notdef
	pMATLAB2Network->m_acftVehicleSend.pos_dX = 0.0;
	pMATLAB2Network->m_acftVehicleSend.pos_dY = 0.0;
	pMATLAB2Network->m_acftVehicleSend.pos_dZ = 0.0;
	pMATLAB2Network->m_acftVehicleSend.rot_dX = 0.0;
	pMATLAB2Network->m_acftVehicleSend.rot_dY = 0.0;
	pMATLAB2Network->m_acftVehicleSend.rot_dZ = 0.0;
#endif	//#ifdef notdef


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

	double dDeltaTime = pMATLAB2Network->dGetDT();
	pMATLAB2Network->m_acftVehicleSend.dt += dDeltaTime;

	prealOutputs[outElapsedTime] = pMATLAB2Network->dGetSimulationTime();

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

		// send the data
		if(!pMATLAB2Network->bSendData())
		{
			ssSetErrorStatus(S,"AVDSNetworkSendBlock:: Error encountered while sending data.\n");
			return;
		}
		pMATLAB2Network->m_acftVehicleSend.dt = 0.0;
	}
}



#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


