 /*      
 *          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. 
 */

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


#include <usr_cblock.h>
#include <CodeBlockData.h>
#include <AVDS2MATLABDef.h>


extern "C"
{
	int iInitTransfer(int iID,int iSize);
	void DeleteTransferAll();
	void TransferSetData(int iClassIndex,int iIndex,double dValue);
	double dTransferGetData(int iClassIndex,int iIndex);
	void TransferReadData(int iClassIndex);
	void TransferWriteData(int iClassIndex);

	void InitializeAC( double *input, double *output, double *param);
	void UpdateAC(double *input, double *output, double *param);
}

extern "C"
{
	int nBlocks = 1;
}


extern "C"
{
/* Add declaration of code blocks */
 CBlock MATLAB_FCS = {
	USR_FCS,				/* Type */
	"MATLAB FCS",		/* Description */
						/* Inputs (from AVDS to MATLAB)*/
	"StickPitch StickRoll StickYaw StickThrottle P Q R  ElapsedTime \
ToMATLAB01 ToMATLAB02 ToMATLAB03 ToMATLAB04 ToMATLAB05 ToMATLAB06 ToMATLAB07 ToMATLAB08 ToMATLAB09 ToMATLAB10",
						/* Outputs (from MATLAB to AVDS)*/
	"ElevatorCmd AileronCmd RudderCmd ThrottleCmd \
ToAVDS01 ToAVDS02 ToAVDS03 ToAVDS04 ToAVDS05 ToAVDS06 ToAVDS07 ToAVDS08 ToAVDS09 ToAVDS10",
	"",					/* Params */
	60,					/* Update Rate (Hz) */
	(void (__cdecl *)(void))InitializeAC,			/* Init Function */
	(void (__cdecl *)(void))UpdateAC			/* Update Function */
};
}

extern "C"
{
/* Add code blocks to execution list */
CBlock *pCBlockx[] = {&MATLAB_FCS};
}




static int iLocalErrorAC = 0;
static int iIndexToAVDS_FCS = -1;
static int iIndexToMATLAB_FCS = -1;

void
InitializeAC( double *input, double *output, double *param)
{
	int i;

	DeleteTransferAll();	/* clear old version of transfer arrays, if they exist */
	iIndexToAVDS_FCS = iInitTransfer(ID_TO_AVDS_FCS,TO_AVDS_TOTAL_FCS);
	iIndexToMATLAB_FCS = iInitTransfer(ID_TO_MATLAB_FCS,TO_MATLAB_TOTAL_FCS);
	if((iIndexToAVDS_FCS==-1)||(iIndexToMATLAB_FCS==-1))
	{
		iLocalErrorAC = 1;
		return;
	}
	else
	{
		iLocalErrorAC = 0;
	}
	// send data to MATLAB
	TransferSetData(iIndexToMATLAB_FCS,TO_MATLAB_RUNNING_FCS,SIMULATION_RUNNING);
	for(i=1;i<TO_MATLAB_TOTAL_FCS;i++)	/* TO_MATLAB_RUNNING_AC==0 */
	{
		TransferSetData(iIndexToMATLAB_FCS,i,input[i-1]);
	}
	TransferWriteData(iIndexToMATLAB_FCS);

	// receive data from MATLAB
	TransferReadData(iIndexToAVDS_FCS);
	for(i=0;i<TO_AVDS_TOTAL_FCS;i++)
	{
		output[i] = dTransferGetData(iIndexToAVDS_FCS,i);
	}
	output[TO_AVDS_THROTTLE_CMD] *= 250.0;	/* throttle command is between 0 and 1 */
}


void
UpdateAC( double *input, double *output, double *param)
{
	int i;

	if(!iLocalErrorAC)
	{
		// send data to MATLAB
		TransferSetData(iIndexToMATLAB_FCS,TO_MATLAB_RUNNING_FCS,SIMULATION_RUNNING);
		for(i=1;i<TO_MATLAB_TOTAL_FCS;i++)	/* TO_MATLAB_RUNNING_AC==0 */
		{
			TransferSetData(iIndexToMATLAB_FCS,i,input[i-1]);
		}
		TransferWriteData(iIndexToMATLAB_FCS);


		// receive data from MATLAB
		TransferReadData(iIndexToAVDS_FCS);
		for(i=0;i<TO_AVDS_TOTAL_FCS;i++)
		{
			output[i] = dTransferGetData(iIndexToAVDS_FCS,i);
		}
		output[TO_AVDS_THROTTLE_CMD] *= 250.0;	/* throttle command is between 0 and 1 */
	}
}


void CBExit(CodeBlockData* codeblockData)
{
	// send zeros MATLAB
	for(int i=1;i<TO_MATLAB_TOTAL_FCS;i++)	/* TO_MATLAB_RUNNING_AC==0 */
	{
		TransferSetData(iIndexToMATLAB_FCS,i,0.0);
	}
	// send data to MATLAB
	TransferSetData(iIndexToMATLAB_FCS,TO_MATLAB_RUNNING_FCS,SIMULATION_STOPPED);
	TransferWriteData(iIndexToMATLAB_FCS);
	DeleteTransferAll();
}



