/**
* \file get_mex_arguments.cpp
* \author basile.graf@epfl.ch
* 
* This file contains functions for getting parameters from MATLAB
*/

/*  //includes dŽplacŽs dans get_mex_arguments.h
#include "math.h"
#include "mex.h"  
#include "string.h"

#include "Datas.h"
*/
#include "get_mex_arguments.h"


#ifndef PI_DEF
#define pi 3.141592653589793
#define PI 3.141592653589793
#endif


void initParameters(struct parameters *P) //default values
{
	int i;
		//P->x0 = 0;  //unused camera stuff
		//P->y0 = 0;
		//P->z0 = 0;
		//P->alpha_cam = 0;
		//P->beta_cam = 0;
		//P->x_rel = 0;
		//P->y_rel = 0;
		//P->z_rel = 0;
		//P->Gain = 0;
		//P->time_stop = 0;
		//P.param={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; //compilateur aime pas ça!! Quel c**!!
		for (i=0; i<PARAMNUM; i++) P->param[i] = 0;
		for (i=0; i<STATENUM; i++) P->abstol[i] = (1e-6);
		P->reltol = (1.0e-4);
	return;
}

void dummy_initParameters(struct parameters *P) //default values
{
	int i;
		P->param[0] = 1.9613;
		P->param[1] = 0.017;
		P->param[2] = 1.941;
		P->param[3] = 0.014;
		P->param[4] = 0.2291;
		P->param[5] = 1.2938;
		P->param[6] = 42.5347;
		P->param[7] = 20.0;
		P->param[8] = 2.0915;
		P->param[9] = 1.0;
		for (i=0; i<STATENUM; i++) P->abstol[i] = (1e-6);
		P->reltol = (1.0e-4);
	return;
}





void get_Parameters(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[], struct parameters *P)
{
	int NStructElems, nfields, pStructIndex, ifield, i;
	const char **fnames; 
	mxArray *fieldsStruct;
	double* temp;
	long rowLen, colLen;

	//if (nrhs==4)
		pStructIndex=2;
	//else if (nrhs==3)
	//	pStructIndex=2;
	//else
	//	mexErrMsgTxt("Number of argument must be 3 or 4");

	if (nrhs<3) mexErrMsgTxt("Number of argument must be 3 or 4");
	
	nfields = mxGetNumberOfFields(prhs[pStructIndex]);
	NStructElems = mxGetNumberOfElements(prhs[pStructIndex]);


	fnames = (const char**)mxCalloc(nfields, sizeof(*fnames));
	for (ifield=0; ifield<nfields; ifield++)
	{
		fnames[ifield] = mxGetFieldNameByNumber(prhs[pStructIndex],ifield);
		/*if (strcmp(fnames[ifield],"x0")==0)  //unused camera stuff
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "x0");
			P->x0 = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"y0")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "y0");
			P->y0 = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"z0")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "z0");
			P->z0 = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"alpha_cam")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "alpha_cam");
			P->alpha_cam = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"beta_cam")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "beta_cam");
			P->beta_cam = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"x_rel")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "x_rel");
			P->x_rel = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"y_rel")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "y_rel");
			P->y_rel = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"z_rel")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "z_rel");
			P->z_rel = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"Gain")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "Gain");
			P->Gain = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"time_stop")==0)
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "time_stop");
			P->time_stop = *mxGetPr(fieldsStruct);
		}*/
		if (strcmp(fnames[ifield],"reltol")==0) //scalar
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "reltol");
			P->reltol = *mxGetPr(fieldsStruct);
		}
		if (strcmp(fnames[ifield],"abstol")==0) //vector
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "abstol");
			temp=mxGetPr(fieldsStruct);
			rowLen = (long) mxGetN(fieldsStruct);
			colLen = (long) mxGetM(fieldsStruct);
			if (rowLen*colLen == 1)
				for (i=0; i<STATENUM; i++) P->abstol[i] = temp[0];
			else if (rowLen*colLen == STATENUM)
				for (i=0; i<STATENUM; i++) P->abstol[i] = temp[i];
			else
				mexErrMsgTxt("P.abstol must be a scalar, a vector of length 12, or not given (default)");
		}
		if (strcmp(fnames[ifield],"param")==0) //vector
		{
			fieldsStruct = mxGetField(prhs[pStructIndex], 0, "param");
			temp=mxGetPr(fieldsStruct);
			for (i=0; i<PARAMNUM; i++) P->param[i] = temp[i];
		}

	}

	return;
}






double* get_state(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	double *temp, *state;
	int rowLen, colLen, k;
	rowLen = mxGetN(prhs[1]);
	colLen = mxGetM(prhs[1]);
	if (!((rowLen==STATENUM && colLen==1) || (rowLen==1 && colLen==STATENUM))) 
		mexErrMsgTxt("Second argument (state) must be a 12 elements vector");
	temp = mxGetPr(prhs[1]);

	//We don't want to write over the passed argument => allocate memory and copy. Don't forget to delete!
	state = new double[rowLen*colLen];
	for (k=0;k<rowLen*colLen;k++) state[k]=temp[k];
	
	return state;
}
//DUMMY VERSION:
double* dummy_get_state()
{
	int k;
	//double state[12];// = {0.0, 0.0 ,1.941 ,0.014 ,0.0 ,0.0 ,20.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0};
	double* state = new double[12];	
	state[0] = 0.0;
	state[1] = 0.0;
	state[2] = 1.941;
	state[3] = 0.014;
	state[4] = 0.0;
	state[5] = 0.0;
	state[6] = 20.0;
	state[7] = 0.0;
	state[8] = 0.0;
	state[9] = 0.0;
	state[10] = 0.0;
	state[11] = 0.0;
	
	fprintf( stderr, "state dans la fonc: \n");
	for (k=0; k<12; k++) fprintf( stderr,"in f: %f \n", float(state[k]));
	
	return state;
}


void delete_state(double *state)
{
	delete state;
}



char* get_flag(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	char *flag;
	int flaglen;

	if (nrhs==3) 
	{
		flag = (char*) mxCalloc(1, sizeof(char));//strcpy_s(flag, defFlag);
		//strcpy_s(flag,1, "");
	
	}
	else if (nrhs==4 || nrhs==5)
	{
		flaglen = mxGetN(prhs[2])+1;
		flag = (char*) mxCalloc(flaglen, sizeof(char)); //mxCalloc is similar to malloc in C
		mxGetString(prhs[2],flag,flaglen);
            
	}
	else
	{
		mexErrMsgTxt("Need 3 or 4 or 5 arguments!");
	}

	return flag;
}





void get_joystick_parameters(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[], struct joy_parameters *jP)
{
	int NStructElems, nfields, pStructIndex, ifield, i;
	const char **fnames; 
	mxArray *fieldsStruct;
	double* temp;

	//default:
	jP->dirX = 1.0;
	jP->dirY = 1.0;
	jP->dirZ = 1.0;
	jP->dirR = 1.0;

	jP->doX = TRUE;
	jP->doY = TRUE;
	jP->doZ = TRUE;
	jP->doR = TRUE;

	//If joystick parameters gived by MATLAB:
	if (nrhs==4)
	{
		pStructIndex=3;

		nfields = mxGetNumberOfFields(prhs[pStructIndex]);
		NStructElems = mxGetNumberOfElements(prhs[pStructIndex]);


		fnames = (const char**)mxCalloc(nfields, sizeof(*fnames));
		for (ifield=0; ifield<nfields; ifield++)
		{
			fnames[ifield] = mxGetFieldNameByNumber(prhs[pStructIndex],ifield);

			if (strcmp(fnames[ifield],"dirX")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "dirX");
				jP->dirX = (float) *mxGetPr(fieldsStruct);
			}
			if (strcmp(fnames[ifield],"dirY")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "dirY");
				jP->dirY = (float) *mxGetPr(fieldsStruct);
			}
			if (strcmp(fnames[ifield],"dirZ")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "dirZ");
				jP->dirZ = (float) *mxGetPr(fieldsStruct);
			}
			if (strcmp(fnames[ifield],"dirR")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "dirR");
				jP->dirR = (float) *mxGetPr(fieldsStruct);
			}

			if (strcmp(fnames[ifield],"doX")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "doX");
				jP->doX = (bool) *mxGetPr(fieldsStruct);
			}
			if (strcmp(fnames[ifield],"doY")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "doY");
				jP->doY = (bool) *mxGetPr(fieldsStruct);
			}
			if (strcmp(fnames[ifield],"doZ")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "doZ");
				jP->doZ = (bool) *mxGetPr(fieldsStruct);
			}
			if (strcmp(fnames[ifield],"doR")==0)
			{
				fieldsStruct = mxGetField(prhs[pStructIndex], 0, "doR");
				jP->doR = (bool) *mxGetPr(fieldsStruct);
			}
		}
	}
	return;
}



void get_time(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[], ODE_data *f_dat)
{
	long rowLen, colLen;
	rowLen = (long) mxGetN(prhs[0]);
	colLen = (long) mxGetM(prhs[0]);

	if (rowLen!=1 || colLen!=1) mexErrMsgTxt("First argument must be a scalar:\n 0 => simulate without returning anything\n t => simulate and give state vector back from time 0 to t");

	f_dat->time_param = *mxGetPr(prhs[0]);
}