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

#define RAD2DEG (57.29577951308232)

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_AC = {
	USR_AC,				/* Type */
	"MATLAB AC",		/* Description */
						/* Inputs (from AVDS to MATLAB)*/
	"dElevator dAileron dRudder dThrottle ElapsedTime \
ToMATLAB01 ToMATLAB02 ToMATLAB03 ToMATLAB04 ToMATLAB05 ToMATLAB06 ToMATLAB07 ToMATLAB08 ToMATLAB09 ToMATLAB10",
						/* Outputs (from MATLAB to AVDS)*/
	"xpos ypos zpos xrot yrot zrot alpha beta G Velocity M DeltaRudder DeltaElevator DeltaAileron eng \
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_AC};
}




static int iLocalErrorAC = 0;
static int iIndexToAVDS_AC = -1;
static int iIndexToMATLAB_AC = -1;

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

	DeleteTransferAll();	/* clear old version of transfer arrays, if they exist */
	iIndexToAVDS_AC = iInitTransfer(ID_TO_AVDS_AC,TO_AVDS_TOTAL_AC);
	iIndexToMATLAB_AC = iInitTransfer(ID_TO_MATLAB_AC,TO_MATLAB_TOTAL_AC);
	if((iIndexToAVDS_AC==-1)||(iIndexToMATLAB_AC==-1))
	{
		iLocalErrorAC = 1;
		return;
	}
	else
	{
		iLocalErrorAC = 0;
	}
	// send data to MATLAB
	TransferSetData(iIndexToMATLAB_AC,TO_MATLAB_RUNNING_AC,SIMULATION_RUNNING);
	for(i=1;i<TO_MATLAB_TOTAL_AC;i++)	/* TO_MATLAB_RUNNING_AC==0 */
	{
		TransferSetData(iIndexToMATLAB_AC,i,input[i-1]);
	}
	TransferWriteData(iIndexToMATLAB_AC);

	// receive data from MATLAB
	TransferReadData(iIndexToAVDS_AC);
	for(i=0;i<TO_AVDS_TOTAL_AC;i++)
	{
		output[i] = dTransferGetData(iIndexToAVDS_AC,i);
	}
		output[TO_AVDS_PHI_RAD] *= RAD2DEG;	/* AVDS input is in degrees */
		output[TO_AVDS_THETA_RAD] *= RAD2DEG;	/* AVDS input is in degrees */
		output[TO_AVDS_PSI_RAD] *= RAD2DEG;	/* AVDS input is in degrees */
		output[TO_AVDS_ALTITUDE_FEET] *= -1.0;	/* AVDS input is down */
}


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

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

		// receive data from MATLAB
		TransferReadData(iIndexToAVDS_AC);
		for(i=0;i<TO_AVDS_TOTAL_AC;i++)
		{
			output[i] = dTransferGetData(iIndexToAVDS_AC,i);
		}
		output[TO_AVDS_PHI_RAD] *= RAD2DEG;	/* AVDS input is in degrees */
		output[TO_AVDS_THETA_RAD] *= RAD2DEG;	/* AVDS input is in degrees */
		output[TO_AVDS_PSI_RAD] *= RAD2DEG;	/* AVDS input is in degrees */
		output[TO_AVDS_ALTITUDE_FEET] *= -1.0;	/* AVDS input is down */
	}
}


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

