 /*      
 *          Copyright (C) 2000 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: 2000/12/27 00:00:00 $

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



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

typedef std::map<int,CMATLAB2Network> MCMATLAB2Network;
static MCMATLAB2Network mpmatlab2Network;

enum enCallType {enInitializeVehicle=1,enSendData,enReceiveData,enCloseConnection,enOpenConnection,enNumberModesPlusOne}; 
enum enSendDataIndex {en_pos_X,en_pos_Y,en_pos_Z,en_rot_X,en_rot_Y,en_rot_Z,en_elev,en_ail,en_rud,en_engine,
						en_pos_dX,en_pos_dY,en_pos_dZ,en_rot_dX,en_rot_dY,en_rot_dZ,enSizeSendData}; 
enum enInitializeDataIndex {enCraftType,enInitLongitudeDegDecimal,enInitLatitudeDegDecimal,enInitAltitudeFeet,enSizeInitData}; 

void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray* prhs[])
{
	char caErrorString[512];
	int iMode = 0;	//this is a user input, used to decide which function to call
	BOOL bError = FALSE;


	/////////////////////////////////////////////
	/*  check for proper number of arguments */
	/////////////////////////////////////////////

	// check right-hand side arguments
	switch(nrhs)
	{
	case 1:		// mode, (close,receive,open) 
	case 2:		// mode, ConnectionID () 
	case 3:		// mode, ConnectionID,data (send,vector)
	case 6:		// mode, ConnectionID,AVDS ID,Size of ToAVDS vector,MATLAB ID,Size of ToMATLAB vector (open,int iToAVDSID,int iNumberToAVDS,int iToMATLABID,int iNumberToMATLAB)
		/* Get the mode. */
		if( !mxIsNumeric(prhs[0]) || !mxIsDouble(prhs[0]) ||
			mxIsEmpty(prhs[0]) || mxIsComplex(prhs[0]) ||
			mxGetN(prhs[0])*mxGetM(prhs[0])!=1 ) 
		{
			mexErrMsgTxt("NetworkAVDS:: Operating mode (first input) must be a scalar.\n");
		}
		iMode = mxGetScalar(prhs[0]);
		bError = FALSE;
		switch(iMode)
		{
		case enInitializeVehicle:
			if((nrhs > 6)||(nrhs <= 0))
			{
				bError = TRUE;
			}
			break;
		case enSendData:
			if(nrhs != 3)
			{
				bError = TRUE;
			}
			break;
		case enReceiveData:
			if(nrhs != 1)
			{
				bError = TRUE;
			}
			break;
		case enOpenConnection:
			if(nrhs != 1)
			{
				bError = TRUE;
			}
			break;
		case enCloseConnection:
			if(nrhs != 1)
			{
				bError = TRUE;
			}
			break;
		default:
			bError = TRUE;
			break;
		}
		if(bError)
		{
			sprintf(caErrorString,"NetworkAVDS:: Wrong number of inputs encoutered for the NetworkAVDS MEX function mode selected: Mode Selected = %d Number of Inputs = %d \n Please see the file 'ReadMe.txt' for more information.\n",iMode,nrhs);
			mexErrMsgTxt(caErrorString);
		}
		break;
	default:
		sprintf(caErrorString,"NetworkAVDS:: Wrong number of inputs encoutered while executing the NetworkAVDS MEX function: %d inputs \n Must be 1, 2 or 5, please see the file 'ReadMe.txt' for more information.\n",nrhs);
		mexErrMsgTxt(caErrorString);
		break;
	}

	// check left-hand side arguments
	switch(nlhs)
	{
	case 0:		
		break;
	case 1:		// (receive)
		if(iMode != enReceiveData)
		{
			sprintf(caErrorString,"NetworkAVDS:: Wrong number of outputs encoutered while executing the NetworkAVDS MEX function: %d inputs \n Must be 0, please see the file 'ReadMe.txt' for more information.\n",nrhs);
			mexErrMsgTxt(caErrorString);
		}
		break;
	default:
		sprintf(caErrorString,"NetworkAVDS:: Wrong number of outputs encoutered while executing the NetworkAVDS MEX function: %d inputs \n Must be 0 or 1, please see the file 'ReadMe.txt' for more information.\n",nrhs);
		mexErrMsgTxt(caErrorString);
		break;
	}

	/////////////////////////////////////////////
	/* execute the functions */
	/////////////////////////////////////////////

	if ((iMode == 0) || (iMode >= enNumberModesPlusOne))
	{
		sprintf(caErrorString,"NetworkAVDS:: Operating Mode must be a scalar between 1 and %d.\nMode number input was found to be %d. , please see the file 'ReadMe.txt' for more information\n",enNumberModesPlusOne-1,iMode);
		mexErrMsgTxt(caErrorString);
	}
	else	//if ((iMode == 0) || (iMode >= enNumberModesPlusOne))
	{

		int iIndexArg = 0;	// temporary storage for argument numbers

		// get Connection ID
		int iIDConnection = -1;
		if((iMode == enInitializeVehicle)||(iMode == enSendData))
		{
			iIndexArg = 1;
			if( !mxIsNumeric(prhs[iIndexArg]) || !mxIsDouble(prhs[iIndexArg]) ||
				mxIsEmpty(prhs[iIndexArg]) || mxIsComplex(prhs[iIndexArg]) ||
				mxGetN(prhs[iIndexArg])*mxGetM(prhs[iIndexArg])!=1 ) 
			{
				mexErrMsgTxt("NetworkAVDS:: Error reading connection ID number.\n");
			}
			iIDConnection = mxGetScalar(prhs[iIndexArg]);
		}
		MCMATLAB2Network::iterator itMATLAB2Network;

		switch(iMode)
		{
		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:

			break;
		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);
				}
				// X and Y axes are swaped!!!
				int iInputNum = en_pos_Y;
				pmatlab2Network->m_acftVehicleSend.pos_X = pdSendData[iInputNum];
				iInputNum = en_pos_X;
				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_Y;
				pmatlab2Network->m_acftVehicleSend.rot_X = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_X;
				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_dY;
				pmatlab2Network->m_acftVehicleSend.pos_dX = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_pos_dX;
				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_dY;
				pmatlab2Network->m_acftVehicleSend.rot_dX = (float)(iInputLength>iInputNum)?(pdSendData[iInputNum]):(0.0);
				iInputNum = en_rot_dX;
				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;
		case enReceiveData:	//NetworkAVDS();
			// [ReceiveMatrix] = NetworkAVDS(enReceiveData);
			{
				if(mpmatlab2Network.empty())
				{
					CMATLAB2Network mpmatlab2NetworkTemp;
					std::pair<MCMATLAB2Network::iterator,bool> prMATLAB2Network = mpmatlab2Network.insert(MCMATLAB2Network::value_type(iIDConnection,mpmatlab2NetworkTemp));
					if(!prMATLAB2Network.second)
					{
						sprintf(caErrorString,"NetworkAVDS:: Error encountered while building the connection for receiving.\n");
						mexErrMsgTxt(caErrorString);
					}
				}
				itMATLAB2Network = mpmatlab2Network.begin();
				CMATLAB2Network* pmatlab2Network = &(*itMATLAB2Network).second;

				// 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())
				{
					//  set the output pointer to the output matrix
					plhs[0] = mxCreateDoubleMatrix(sizeof(Aircraft)/8,1, mxREAL);
					//  create a pointer to a copy of the output matrix 
					double* pdReceiveData = mxGetPr(plhs[0]);
					Aircraft* pacftTemp = &(*pmatlab2Network->m_lacftVehiclesReceive.begin());
					int iCount = 0;
					pdReceiveData[iCount++] = pacftTemp->state;
					pdReceiveData[iCount++] = pacftTemp->elev;
					pdReceiveData[iCount++] = pacftTemp->ail;
					pdReceiveData[iCount++] = pacftTemp->rud;
					pdReceiveData[iCount++] = pacftTemp->throttle;
					pdReceiveData[iCount++] = pacftTemp->u;
					pdReceiveData[iCount++] = pacftTemp->v;
					pdReceiveData[iCount++] = pacftTemp->w;
					pdReceiveData[iCount++] = pacftTemp->p;
					pdReceiveData[iCount++] = pacftTemp->q;
					pdReceiveData[iCount++] = pacftTemp->r;
					pdReceiveData[iCount++] = pacftTemp->ref_X;
					pdReceiveData[iCount++] = pacftTemp->ref_Y;
					pdReceiveData[iCount++] = pacftTemp->pos_X;
					pdReceiveData[iCount++] = pacftTemp->pos_Y;
					pdReceiveData[iCount++] = pacftTemp->pos_Z;
					pdReceiveData[iCount++] = pacftTemp->pos_dX;
					pdReceiveData[iCount++] = pacftTemp->pos_dY;
					pdReceiveData[iCount++] = pacftTemp->pos_dZ;
					pdReceiveData[iCount++] = pacftTemp->rot_X;
					pdReceiveData[iCount++] = pacftTemp->rot_Y;
					pdReceiveData[iCount++] = pacftTemp->rot_Z;
					pdReceiveData[iCount++] = pacftTemp->rot_dX;
					pdReceiveData[iCount++] = pacftTemp->rot_dY;
					pdReceiveData[iCount++] = pacftTemp->rot_dZ;
					pdReceiveData[iCount++] = pacftTemp->engine;
					pdReceiveData[iCount++] = pacftTemp->hostid;
					//pdReceiveData[iCount++] = packet;
					pdReceiveData[iCount++] = pacftTemp->dt;
					pmatlab2Network->m_lacftVehiclesReceive.pop_front();
				}
				else	//if (!pmatlab2Network->m_lacftVehiclesReceive.empty())
				{
					// No packets received, returning NULL
					//  set the output pointer to the output matrix
					plhs[0] = mxCreateDoubleMatrix(0,1, mxREAL);
				}

			}	//case enReceiveData:
			break;
		case enOpenConnection:	//NetworkAVDS();
			// NetworkAVDS(enOpenConnection);
			{
				iIDConnection = 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->bOpenConnection())
				{
					sprintf(caErrorString,"NetworkAVDS:: Error encountered while opening the connection.\n");
					mexErrMsgTxt(caErrorString);
				}
			}
			break;
		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;
		default:
			sprintf(caErrorString,"NetworkAVDS:: Unknown mode encoutnered, mode: %d \n ",iMode);
			mexErrMsgTxt(caErrorString);
			break;
		}	//switch(iMode)
	}	//if ((iMode == 0) || (iMode >= enNumberModesPlusOne))
}



