#include <string.h> // for memmove (microsoft bug)
#include <math.h>

#ifdef my_ctrl_stand_alone 
#include "ICsimstruc.h"
#else
#include "simstruc.h"
#endif

#include "mex.h"
#include "ICctrl_classes.h"

extern int save;
extern debugClass *d_bug;
extern mimo *cntrl;
extern mimo *plant;
extern SimStruct *S;

/*==================general funtions===============================*/

void error(char *s)
{
   static char msg[256];   //ILLEGAL: to fix use "static char msg[256];"
   sprintf(msg,"Error due to %s.", s);
   ssSetErrorStatus(S,msg);
   return;
}

double GetDouble(const mxArray *mx,char *VariableName)
{
    const mxArray *pa;
    char buf[100];
    pa=mxGetField(mx,0,VariableName);
    if (pa==NULL)
    {    
        sprintf((char*)&buf,"variable '%s.%s' is missing",
                 mxGetName(mx),VariableName);
        error(buf);
    };
    return mxGetScalar(pa);
};

double GetDoublePlus(const mxArray *mx,char *VariableName, double defaultvalue)
{
    const mxArray *pa;    
    pa=mxGetField(mx,0,VariableName);
    if (pa==NULL) return defaultvalue;
    return mxGetScalar(pa);              
};

vector getVector(const mxArray *mx,char *subindex)
{
    vector v;
    const mxArray *pa;
    char buf[100];
    pa=mxGetField(mx,0,subindex);
    if (pa==NULL)
    {    
        sprintf((char*)&buf,"variable '%s.%s' is missing",
                 mxGetName(mx),subindex);
        error(buf);
    }
    v.p=(dataPtr)mxGetPr(pa);
    v.n=mxGetNumberOfElements(pa);
	v.extend=0;
    return v;
};

table getTable(const mxArray *mx,char *subindex) // !!!!!! attention: when you use this function, to get a table,
												// you must make "mxFree(table->p)" before exiting the program.
{
	table X;
	vector Xvec=getVector(mx,subindex);
	int i,m;

	m=X.m=mxGetM(mxGetField(mx,0,subindex));
	X.n=Xvec.n/X.m;	X.extend=0;	X.p=(dataPtr*)mxCalloc(X.n,sizeof(double*));
	mexMakeMemoryPersistent(X.p);
	for (i=0; i<X.n; i++, Xvec.p+=m) X.p[i]=Xvec.p;
	return X;
};

/* =======================table object=============================*/

table *t_Init(table *_thisref,int _m,int _n) //m=ligne; n=colonne
{
    table *thisref;
	dataPtr t;
	int i;
    if (_thisref==NULL)
    {
        thisref=(table*)mxMalloc(sizeof(table));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
    thisref->n=_n; thisref->m=_m; thisref->extend=0;
	t=(dataPtr)mxMalloc(_n*_m*sizeof(double));
	mexMakeMemoryPersistent(t);
	thisref->p=(dataPtr*)mxMalloc(_n*sizeof(double*));
    mexMakeMemoryPersistent(thisref->p);
	for (i=0; i<_n; i++, t+=_m) thisref->p[i]=t;
    return thisref;
};

void t_extend(table *thisref)
{
	int n=thisref->n,m=thisref->m,i;
	dataPtr tmp,tmp2,tmp3;
	if (thisref->extend==0)
	{	
		tmp=(dataPtr)mxMalloc((n*(m+100))*sizeof(double));
		mexMakeMemoryPersistent(tmp);
		if (tmp==NULL) error("memory allocation error");
		tmp2=tmp; tmp3=*thisref->p;
		for (i=0; i<n; i++, tmp2+=(m+100)*n) thisref->p[i]=tmp2;
		tmp2=*thisref->p;
		for (i=0; i<n; i++, tmp+=(m+100)*n, tmp2+=m*n) memmove(tmp,tmp2,m*n*sizeof(double));
		mxFree(tmp3);
		thisref->extend=100;
	} else thisref->extend--;
	thisref->m++;
};

void t_exactshape(table *thisref)
{
	int n=thisref->n,m=thisref->m,i;
	dataPtr tmp,tmp2;
	if (thisref->extend!=0)
	{			
		tmp2=*thisref->p;
		for (i=0; i<n; i++, tmp2+=m*n) thisref->p[i]=tmp2;
		tmp=tmp2=*thisref->p;
		for (i=0; i<n; i++, tmp2+=(m+thisref->extend)*n, tmp+=m*n) memmove(tmp,tmp2,m*n*sizeof(double));
		tmp=(dataPtr)mxRealloc(*thisref->p,n*m*sizeof(double));		
		thisref->extend=0;
	};
};

void t_Terminate(table *thisref)
{
    mxFree(thisref->p[0]); mxFree(thisref->p);
};

/* =======================vector object=============================*/

vector *v_Init(vector *_thisref,int _n)
{
    vector *thisref;
    if (_thisref==NULL)
    {
        thisref=(vector*)mxMalloc(sizeof(vector));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
	thisref->extend=0;
    thisref->n=_n; thisref->p=(dataPtr)mxMalloc(_n*sizeof(double)); 
    mexMakeMemoryPersistent(thisref->p);
    return thisref;
};

void v_extend(vector *thisref)
{	
	if (thisref->extend==0)
	{
		thisref->p=(dataPtr)mxRealloc(thisref->p,(thisref->n+100)*sizeof(double));
/*??*/  mexMakeMemoryPersistent(thisref->p);
		if (thisref->p==NULL) error("memory allocation error");
		thisref->extend=100;
	} else thisref->extend--;
	thisref->n++;
};

void v_exactshape(vector *thisref)
{
	int n=thisref->n;
	if (thisref->extend!=0)
	{
		thisref->p=(dataPtr)mxRealloc(thisref->p,n*sizeof(double));
		thisref->extend=0;
	};
};

void v_Terminate(vector *thisref)
{
    mxFree(thisref->p);
};

/* =======================vectorI object=============================*/

vectorI *vi_Init(vectorI *_thisref,int _n)
{
    vectorI *thisref;
    if (_thisref==NULL)
    {
        thisref=(vectorI*)mxMalloc(sizeof(vectorI));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
	thisref->extend=0;
    thisref->n=_n; thisref->p=(dataPtrI)mxMalloc(_n*sizeof(int));
    mexMakeMemoryPersistent(thisref->p);
    return thisref;
};

void vi_Terminate(vectorI *thisref)
{
    mxFree(thisref->p);
};

/* =======================mimo object===============================*/

syst *mimo_get(mimo *thisref,int indice)
{
   return (*thisref->head)[thisref->links.p[indice<<1]];
};

void mimo_Terminate(mimo *thisref)
{
    int i;
    syst *tamp;

	vi_Terminate(&thisref->links);
	vi_Terminate(&thisref->indiceParams);
    for (i=0; i<thisref->nMapping; i++)
    {
        tamp=(*thisref->head)[i];
        syst_Terminate(tamp);
        mxFree(tamp);
    };
};

double mimo_eval(mimo *thisref,int nsyst,vector *in)
{
	return syst_eval((*thisref->head)[thisref->links.p[nsyst<<1]],
				     thisref->links.p[(nsyst<<1)+1],
					 in);
};

double mimo_jacobinput(mimo *thisref,int nsyst,int k, vector *in)
{
	return syst_jacobinput((*thisref->head)[thisref->links.p[nsyst<<1]],
				           thisref->links.p[(nsyst<<1)+1],
						   k,in);
};

double mimo_jacobparam(mimo *thisref,int nsyst,int p,vector *entree_work)
{
	if ((p>=thisref->indiceParams.p[nsyst])&&(p<thisref->indiceParams.p[nsyst+1]))
       return syst_jacobparam((*thisref->head)[thisref->links.p[nsyst<<1]],
	     			          thisref->links.p[(nsyst<<1)+1],
						      p-thisref->indiceParams.p[nsyst],entree_work);
    else 
       return 0;
};

double mimo_getParam(mimo *thisref,int p)
{
    int i=1;
    while ((p>=thisref->indiceParams.p[i])||(mimo_numParams(thisref,i-1)==0)) i++;
    i--;
    return syst_getParam((*thisref->head)[thisref->links.p[i<<1]],
						 p-thisref->indiceParams.p[i]);
};

void mimo_setParam(mimo *thisref,int p,double a)
{
    int i=1;
	while ((p>=thisref->indiceParams.p[i])||(mimo_numParams(thisref,i-1)==0)) i++;
    i--;
    syst_setParam((*thisref->head)[thisref->links.p[i<<1]],
				  p-thisref->indiceParams.p[i],a);
};

int mimo_numParams(mimo *thisref,int nsyst)
{
	return ((*thisref->head)[thisref->links.p[nsyst<<1]])->optimParams.n;
};

mimo * mimo_Init(mimo *_thisref,const mxArray *pa,char *BaseName, char _type)
{
	return mimo_Init2(_thisref,mxGetField(pa,0,BaseName),_type);
};

mimo * mimo_Init2(mimo *_thisref,const mxArray *tmp3,char _type)
{
    const mxArray *system=mxGetField(tmp3,0,"system");
	const mxArray *dyn=mxGetField(tmp3,0,"dynamics");
	const int nMapping=mxGetNumberOfElements(mxGetField(system,0,"mapping"));
    mxArray *tmp2,*tmp;
	vector tlinks,nu,nd,ny;	
    char str[80],buf[100];
    int sortie,i,nOut;
    mimo *thisref;
    if (_thisref==NULL) 
    {
        thisref=(mimo*)mxMalloc(sizeof(mimo));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

    thisref->nIn=(int)GetDouble(system,"n_in");
    thisref->nOut=(int)GetDouble(system,"n_out");
	nOut=thisref->nOut;
    thisref->minmax=getVector(system,"limits");
	thisref->type=_type;

	tlinks=getVector(system,"links");	vi_Init(&thisref->links,tlinks.n);
	for (i=0; i<(tlinks.n>>1); i++) 
	{
		thisref->links.p[i<<1]=(int)tlinks.p[i]-1;
		thisref->links.p[(i<<1)+1]=(int)tlinks.p[i+(tlinks.n>>1)]-1;
	};
	
    thisref->head=(syst *((*)[1]))mxMalloc(thisref->nOut*sizeof(syst*));
	mexMakeMemoryPersistent(thisref->head);

	nu=getVector(dyn,"nu"); 	nu.n=thisref->nIn;
	nd=getVector(dyn,"nd"); 	nd.n=thisref->nIn;
	ny=getVector(dyn,"ny"); 	ny.n=thisref->nOut;

	sortie=0; 
    while (sortie<nMapping)
    {
		tmp=mxGetCell(mxGetField(system,0,"mapping"),sortie);
		tmp2=mxGetField(tmp,0,"n_rules");
		if (tmp2!=NULL)
		{
			mxGetString(mxGetCell(mxGetField(tmp,0,"model_code"),1),(char*)&str,80);
			if (strncmp((char*)&str,"inversedist",11)==0)
				(*thisref->head)[sortie]=(syst*)hyp_Init(NULL,tmp,nu,nd,ny,nOut);
        
			if (strncmp((char*)&str,"gaussian",8)==0)
				(*thisref->head)[sortie]=(syst*)gaus_Init(NULL,tmp,nu,nd,ny,nOut);

			if ((*thisref->head)[sortie]==NULL)
			{    
				sprintf((char*)&buf,"fuzzy set of type '%s' is unknown",
						(char*)&str);
				error(buf);
			};			
		} else
		{
			tmp2=mxGetField(tmp,0,"linears");
			if (tmp2!=NULL)	(*thisref->head)[sortie]=(syst*)linear_Init(NULL,tmp,nu,nd,ny,nOut);
			else
			{
				tmp2=mxGetField(tmp,0,"examples_x");
				if (tmp2!=NULL)	(*thisref->head)[sortie]=(syst*)lazy_Init(NULL,tmp,nu,nd,ny,nOut);
				else
				{
					tmp2=mxGetField(tmp,0,"lookups");
					if	(tmp2!=NULL) (*thisref->head)[sortie]=(syst*)lookup_Init(NULL,tmp,nu,nd,ny,nOut);
					else
					{
						if (((*thisref->head)[sortie]=(syst*)defaultMap_Init(NULL,tmp,nu,nd,ny,nOut))==NULL)
						{
							sprintf((char*)&buf,"unknown mapping %i",sortie+1);
							error(buf);
						};
					};
				};
			};
		};
		sortie++;
		nu.p++;	nd.p++; ny.p++; 
//		nu.p+=thisref->nIn;	nd.p+=thisref->nIn; ny.p+=thisref->nOut; 
    };
	thisref->nMapping=sortie;

    thisref->numTParams=0;
    vi_Init(&thisref->indiceParams,thisref->nOut+1);
    for (i=0; i<thisref->nOut; i++)
    {
        thisref->indiceParams.p[i]=thisref->numTParams;
        thisref->numTParams+=mimo_get(thisref,i)->optimParams.n;
    }
    thisref->indiceParams.p[thisref->nOut]=thisref->numTParams;
    
    return thisref;
};

/* =======================syst object===================================*/

syst * syst_Init(syst *_thisref,const mxArray *pmiso,
					vector nu,vector nd, vector ny, int nOut)
{
    syst *thisref;
	const mxArray *pm=mxGetField(pmiso,0,"mapping");
	vector tmp;
    int i;
    
    if (_thisref==NULL)
    {
        thisref=(syst*)mxMalloc(sizeof(syst));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
    thisref->type='e'; // error : 'syst' type must be derived.
    
    vi_Init(&thisref->nu,nu.n); 
	for (i=0; i<nu.n; i++) thisref->nu.p[i]=(int)nu.p[i*nOut];
	vi_Init(&thisref->nd,nd.n); 
	for (i=0; i<nd.n; i++) thisref->nd.p[i]=(int)nd.p[i*nOut];
	vi_Init(&thisref->ny,ny.n); 
	for (i=0; i<ny.n; i++) thisref->ny.p[i]=(int)ny.p[i*nOut];
	
	tmp=getVector(pm,"optimparams"); vi_Init(&thisref->optimParams,tmp.n);
	for (i=0; i<tmp.n; i++) thisref->optimParams.p[i]=(int)tmp.p[i];

    thisref->nEntree=0;
    for (i=0; i<thisref->ny.n; i++) thisref->nEntree+=thisref->ny.p[i];
    for (i=0; i<thisref->nu.n; i++) thisref->nEntree+=thisref->nu.p[i];

    return thisref;
};

void syst_Terminate(syst *thisref)
{
	switch (thisref->type)
	{
	case 'h': hyp_Terminate((hyperbolic*)thisref); break;
	case 'g': gaus_Terminate((gaussian*)thisref); break;
	case 'l': linear_Terminate((linear*)thisref); break;
	case 'd': defaultMap_Terminate((defaultMap*)thisref); break;
	case 'o': lookup_Terminate((lookup*)thisref); break;
	case 'a': lazy_Terminate((lazy*)thisref); break;
	default: error("unknown syst"); break;
	}
	vi_Terminate(&thisref->optimParams);
	vi_Terminate(&thisref->nu);
	vi_Terminate(&thisref->nd);
	vi_Terminate(&thisref->ny);
};

double syst_eval(syst *thisref,int nsyst, vector *in)
{
	switch (thisref->type)
	{
	case 'h':
	case 'g': return gaus_eval((gaussian*)thisref,in); break;
	case 'l': return linear_eval((linear*)thisref,nsyst,in); break;
	case 'o': return lookup_eval((lookup*)thisref,in); break;
	case 'd': return defaultMap_eval((defaultMap*)thisref,nsyst,in); break;
	case 'a': return lazy_eval((lazy*)thisref,in); break;
	default: error("unknown syst"); break;
	}
	return 0;
};

double syst_jacobinput(syst *thisref,int nsyst, int k, vector *in)
{
	switch (thisref->type)
	{
	case 'h': return hyp_jacobinput((hyperbolic*)thisref,k,in);
	case 'g': return gaus_jacobinput((gaussian*)thisref,k,in);
	case 'l': return linear_jacobinput((linear*)thisref,nsyst,k,in);
	case 'o': return lookup_jacobinput((lookup*)thisref,k,in);
	case 'd': return defaultMap_jacobinput((defaultMap*)thisref,nsyst,k,in);
	case 'a': return lazy_jacobinput((lazy*)thisref,k,in);
	default: error("unknown syst"); break;
	}
	return 0;
};

double syst_jacobparam(syst *thisref,int nsyst,int pa,vector *entree_work)
{
	int p=thisref->optimParams.p[pa];
	switch (thisref->type)
	{
	case 'h':
	case 'g': return gaus_jacobparam((gaussian*)thisref,p,entree_work);
	default: error("unknown syst"); break;
	}
	return 0;
};

double syst_getParam(syst *thisref,int pa)
{
	int p=thisref->optimParams.p[pa];
	switch (thisref->type)
	{
	case 'h':
	case 'g': return gaus_getParam((gaussian*)thisref,p);
	default: error("unknown syst"); break;
	}
	return 0;
};

void syst_setParam(syst *thisref,int pa,double a)
{
	int p=thisref->optimParams.p[pa];
	switch (thisref->type)
	{
	case 'h':
	case 'g': gaus_setParam((gaussian*)thisref,p,a); break;
	default: error("unknown syst"); break;
	}
};

/*======================default mapping object==========================*/

defaultMap* defaultMap_Init(defaultMap *_thisref, const mxArray *pmiso,
							vector nu, vector nd, vector ny, int nOut)
{
    defaultMap *thisref;
	mxArray *in,*nsyst,*plhs[1];
	double *p,*pnsyst;
	int t,i;
    if (_thisref==NULL)
    {
        thisref=(defaultMap*)mxMalloc(sizeof(defaultMap));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
	syst_Init((syst*)thisref,pmiso,nu,nd,ny,nOut);
	thisref->type='d';
	thisref->optimParams.n=0;

	thisref->prhs[0]=(mxArray*)pmiso;
	
	in=mxCreateDoubleMatrix(1,thisref->nEntree,mxREAL); 
	mexMakeArrayPersistent(in);
	p=mxGetPr(in);
	thisref->prhs[1]=in;	
	
	nsyst=mxCreateDoubleMatrix(1,1,mxREAL); 
	mexMakeArrayPersistent(nsyst);
	pnsyst=mxGetPr(nsyst);
	thisref->prhs[2]=nsyst;

	for (i=0; i<thisref->nEntree; i++) p[i]=0;
	*pnsyst=1;

	mexSetTrapFlag(1);
	t=mexCallMATLAB(1, plhs, 3, thisref->prhs, "eval");
	if (!t) 
	{
		mxDestroyArray(plhs[0]);
		t=mexCallMATLAB(1, plhs, 3, thisref->prhs, "jacob_inputs");
		if (!t) 
		{
			mxDestroyArray(plhs[0]);
			mexSetTrapFlag(0);
			return thisref;
		};
	};
	mexSetTrapFlag(0);
	if (_thisref==NULL) mxFree(thisref);
	syst_Terminate((syst*)&thisref);
	return NULL;
};

void defaultMap_Terminate(defaultMap *thisref)
{
	mxDestroyArray(thisref->prhs[1]);
	mxDestroyArray(thisref->prhs[2]);
};

double defaultMap_eval(defaultMap* thisref,int nsyst,vector *in)
{
	mxArray *plhs[1];	
	double *p=mxGetPr(thisref->prhs[1]),*pnsyst=mxGetPr(thisref->prhs[2]),out;

	memcpy(p,in->p,thisref->nEntree*sizeof(double));
	*pnsyst=nsyst+1;
	mexCallMATLAB(1, plhs, 3, thisref->prhs, "eval");
	out=*(mxGetPr(plhs[0]));
	mxDestroyArray(plhs[0]);
	return out;
};

double defaultMap_jacobinput(defaultMap *thisref,int nsyst,int k,vector *in)
{
	mxArray *plhs[1];	
	double *p=mxGetPr(thisref->prhs[1]),*pnsyst=mxGetPr(thisref->prhs[2]),out;

	memcpy(p,in->p,thisref->nEntree*sizeof(double));
	*pnsyst=nsyst+1;
	mexCallMATLAB(1, plhs, 3, thisref->prhs, "jacob_inputs");
	out=(mxGetPr(plhs[0]))[k];
	mxDestroyArray(plhs[0]);
	return out;
};

/* =======================lookup object===================================*/
lookup *lookup_Init(lookup *_thisref, const mxArray *pmiso,vector nu, vector nd, 
                    vector ny, int nOut)
{
    lookup *thisref;
	int nIn,nVertex,i,j;
	const mxArray *tmp=mxGetField(pmiso,0,"scales");
	mxArray *tmp2,*tmp3=NULL;
    if (_thisref==NULL)
    {
        thisref=(lookup*)mxMalloc(sizeof(lookup));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
	syst_Init((syst*)thisref,pmiso,nu,nd,ny,nOut);
	thisref->type='o';
	thisref->optimParams.n=0;
	
	nIn=thisref->nEntree; nVertex=1<<nIn;

	vi_Init(&thisref->dims,nIn);
    thisref->scales.n=nIn; thisref->scales.m=0; thisref->scales.extend=0;	
	thisref->scales.p=(dataPtr*)mxMalloc(nIn*sizeof(double*));
    mexMakeMemoryPersistent(thisref->scales.p);
	for (i=0; i<nIn; i++)
	{
		thisref->dims.p[i]=mxGetN(mxGetCell(tmp,i));
		thisref->scales.p[i]=mxGetPr(mxGetCell(tmp,i));
	};
    thisref->lup=getVector(pmiso,"lookups");
	v_Init(&thisref->levels,nIn);
	vi_Init(&thisref->valLow,nIn);
	vi_Init(&thisref->valHigh,nIn);
	thisref->idxs = (double**) mxCalloc(nVertex,sizeof(double*)); mexMakeMemoryPersistent(thisref->idxs);

	thisref->Y_IN = mxCreateDoubleMatrix(nVertex, nIn+1, mxREAL);
	thisref->Y_OUT = mxCreateDoubleMatrix(nVertex, 1, mxREAL);
	thisref->input_array[0]=thisref->Y_IN;
	thisref->input_array[1]=thisref->Y_OUT;
	mexMakeArrayPersistent(thisref->Y_IN);
	mexMakeArrayPersistent(thisref->Y_OUT);
	thisref->Dlup.p=NULL;

	tmp2=mxGetField(mxGetField(pmiso,0,"mapping"),0,"opt");
	if (tmp2!=NULL) tmp3=mxGetField(tmp2,0,"dlookups");
	if (tmp3!=NULL)
	{		
		thisref->Dlup.n=nIn; thisref->Dlup.m=0; thisref->Dlup.extend=0;	
		thisref->Dlup.p=(dataPtr*)mxMalloc(nIn*sizeof(double*));
		mexMakeMemoryPersistent(thisref->Dlup.p);
		for (i=0; i<nIn; i++)
			thisref->Dlup.p[i]=mxGetPr(mxGetCell(tmp3,i));		

		tmp3=mxGetField(tmp2,0,"dscales");
		vi_Init(&thisref->Ddims,nIn);
		thisref->Dscales.n=nIn; thisref->Dscales.m=0; thisref->Dscales.extend=0;	
		thisref->Dscales.p=(dataPtr*)mxMalloc(nIn*sizeof(double*));
		mexMakeMemoryPersistent(thisref->Dscales.p);
		for (i=0; i<nIn; i++)
		{
			thisref->Ddims.p[i]=mxGetN(mxGetCell(tmp3,i));
			thisref->Dscales.p[i]=mxGetPr(mxGetCell(tmp3,i));
		};
	};
	return thisref;
};

void lookup_Terminate(lookup *thisref)
{
	mxFree(thisref->scales.p);
	mxFree(thisref->idxs);
	mxDestroyArray(thisref->Y_IN);
	mxDestroyArray(thisref->Y_OUT);
	if (thisref->Dlup.p!=NULL)
	{
		mxFree(thisref->Dlup.p);
		mxFree(thisref->Dscales.p);
		vi_Terminate(&thisref->Ddims);
	};
	v_Terminate(&thisref->levels);
	vi_Terminate(&thisref->dims);
	vi_Terminate(&thisref->valLow);
	vi_Terminate(&thisref->valHigh);
};

double mean(double *idxs[],double levels[],int level)
{
	if (level==0) return *(idxs[0]);	
	else return (1-levels[level-1]) * mean(idxs                 ,levels, level-1) + 
		           levels[level-1]  * mean(idxs + (1<<(level-1)),levels, level-1);
};

double lookup_eval(lookup* thisref,vector *in)
{
    double *x=in->p,
	       *lup=thisref->lup.p,
		   *levels=thisref->levels.p;
	double **idxs=thisref->idxs,
		   **scales=thisref->scales.p;
	int *dims=thisref->dims.p,
		*valLow=thisref->valLow.p,
		*valHigh=thisref->valHigh.p;
	int i, j, k,factor,nIn=thisref->nEntree,nVertex=(1<<nIn);

// Find in which hypercube the point is
	for (i=0;i<nIn;i++)
	{
		j = 0;
		
		while ((j<dims[i])&&(scales[i][j]<x[i])) j++;
		
		if (j==0)
		{
			levels[i]=1;
			valLow[i]=0;
			valHigh[i]=0;
		}
		else
		{
			if (j==dims[i])
			{
				levels[i]=0;
				valHigh[i]=--j;
				valLow[i]=j;
			}
			else
			{
				levels[i]=(x[i] - scales[i][j-1])/(scales[i][j] - scales[i][j-1]);
				valHigh[i]=j--;
				valLow[i]=j;
			};
		};
	};
	
	//Get the idxs
	for (i=0;i<nVertex;i++)
	{
		k=0;
		factor=1;
		for (j=0;j<nIn;j++)
		{
			k += factor * (((i&(1<<j))==0)?valLow[j]:valHigh[j]);
			factor *= dims[j];
		}
		idxs[i] = lup + k;
	}
	
	return (1-levels[nIn-1]) * mean(idxs, levels, nIn-1) + levels[nIn-1] * mean(idxs + (1<<(nIn-1)),levels , nIn-1);
};

double lookup_jacobinput(lookup *thisref,int kk,vector *entree)
{
	int i,j,k,factor,
		nIn=thisref->nEntree,
		nVertex=(1<<nIn);
	mxArray *output_array[1];
    double *x=entree->p,
	       *lup=thisref->lup.p,
		   *levels=thisref->levels.p,
		   *in = mxGetPr(thisref->Y_IN),
		   *out = mxGetPr(thisref->Y_OUT);
	double **idxs=thisref->idxs,
		   **scales=thisref->scales.p;
	int *dims=thisref->dims.p,
		*valLow=thisref->valLow.p,
		*valHigh=thisref->valHigh.p;
	double val_OUT;


	if (thisref->Dlup.p==NULL)
	{
		// Find in which hypercube the point is
		for (i=0;i<nIn;i++)
		{		
			j = 0;
			
			while ((j<dims[i])&&(scales[i][j]<x[i])) j++;
			
			if (j==0)
			{
				valLow[i]=0;
				valHigh[i]=1;
			}
			else
			{
				if (j==dims[i])
				{
					valHigh[i]=--j;
					valLow[i]=--j;
				}
				else
				{
					valHigh[i]=j--;
					valLow[i]=j;
				}
			}
		}
		
		//Get the idxs
		for (i=0;i<nVertex;i++)
		{
			k=0;
			factor=1;
			for (j=0;j<nIn;j++)
			{
				in[i+j*nVertex] = scales[j][(((i&(1<<j))==0)?valLow[j]:valHigh[j])];
				k += factor * (((i&(1<<j))==0)?valLow[j]:valHigh[j]);
				factor *= dims[j];
			}
			in[i+nIn*nVertex]=1;
			out[i] = *(lup + k);
		};

		mexCallMATLAB(1, output_array, 2, thisref->input_array, "mldivide");
		val_OUT=(mxGetPr(output_array[0]))[kk];
		mxDestroyArray(output_array[0]);
		return val_OUT;
	} else
	{
		// normal "lookup" but on Dlup[kk], Ddims, Dscale
		dims=thisref->Ddims.p;
		scales=thisref->Dscales.p;
		lup=thisref->Dlup.p[kk];

		for (i=0;i<nIn;i++)
		{
			j = 0;
			
			while ((j<dims[i])&&(scales[i][j]<x[i])) j++;
			
			if (j==0)
			{
				levels[i]=1;
				valLow[i]=0;
				valHigh[i]=0;
			}
			else
			{
				if (j==dims[i])
				{
					levels[i]=0;
					valHigh[i]=--j;
					valLow[i]=j;
				}
				else
				{
					levels[i]=(x[i] - scales[i][j-1])/(scales[i][j] - scales[i][j-1]);
					valHigh[i]=j--;
					valLow[i]=j;
				};
			};
		};
		
		//Get the idxs
		for (i=0;i<nVertex;i++)
		{
			k=0;
			factor=1;
			for (j=0;j<nIn;j++)
			{
				k += factor * (((i&(1<<j))==0)?valLow[j]:valHigh[j]);
				factor *= dims[j];
			}
			idxs[i] = lup + k;
		}
		
		return (1-levels[nIn-1]) * mean(idxs, levels, nIn-1) + levels[nIn-1] * mean(idxs + (1<<(nIn-1)),levels , nIn-1);
	}
};

/* =======================lazzy object====================================*/

lazy *lazy_Init(lazy *_thisref, const mxArray *pmiso,vector nu, vector nd, 
                    vector ny,int nOut)
{
	const mxArray *pa;
	vector range,Xvec;
	int idm,idM,nz,mx,i;	
    lazy *thisref;	
    if (_thisref==NULL)
    {
        thisref=(lazy*)mxMalloc(sizeof(lazy));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
	syst_Init((syst*)thisref,pmiso,nu,nd,ny,nOut);
	thisref->type='a';
	thisref->optimParams.n=0;

	thisref->Y=getVector(pmiso,"examples_y");
    thisref->LAMBDA=GetDoublePlus(pmiso,"lambda",1E6);

	thisref->Wvec.p = NULL;
    pa=mxGetField(pmiso,0,"W");
    if (pa!=NULL)
    {    
	    thisref->Wvec.p=(dataPtr)mxGetPr(pa);
		thisref->Wvec.n=mxGetNumberOfElements(pa);
	};

    // Range identification examples
    range=getVector(pmiso,"id_par");
    
    nz = thisref->nEntree+1;

	thisref->X=getTable(pmiso,"examples_x");
	mx=thisref->X.m;

    idm = (int)range.p[0];
    idM = (int)range.p[1];
    idm = (idm<2)? 2 : idm;
    idM = (idM>mx)? mx : idM;
    idM = (idM<idm)? idm : idM;
	thisref->idm=idm;
	thisref->idM=idM;

	t_Init(&thisref->C,mx,thisref->nEntree);
	t_Init(&thisref->Z,nz,idM);
	t_Init(&thisref->v,nz,nz);
	v_Init(&thisref->t_hat,nz);
	v_Init(&thisref->W,idM);
	v_Init(&thisref->t,nz);
	v_Init(&thisref->tB,nz);
	v_Init(&thisref->a,nz);
	v_Init(&thisref->BestDist,idM+2);
	vi_Init(&thisref->BestIndx,idM+1);
	return thisref;
};

void lazy_Terminate(lazy *thisref)
{
	mxFree(thisref->X.p);
	t_Terminate(&thisref->C);
	t_Terminate(&thisref->Z);
	t_Terminate(&thisref->v);
	v_Terminate(&thisref->t_hat);
	v_Terminate(&thisref->W);
	v_Terminate(&thisref->t);
	v_Terminate(&thisref->tB);
	v_Terminate(&thisref->a);
	v_Terminate(&thisref->BestDist);
	vi_Terminate(&thisref->BestIndx);
};

void lazy_work(lazy* thisref,vector *in)
{
	double INF=mxGetInf(),
		   LAMBDA=thisref->LAMBDA;
	dataPtr *C=thisref->C.p,
			*X=thisref->X.p,
			*Z=thisref->Z.p,
			*v=thisref->v.p;
	dataPtr Wvec=thisref->Wvec.p,
			W=thisref->W.p,
			t=thisref->t.p,
			tB=thisref->tB.p,
			a=thisref->a.p,
			BestDist=thisref->BestDist.p,
			Y=thisref->Y.p,
			Q=in->p,
			Vc=v[0],
			Zvec=Z[0];
	dataPtrI BestIndx=thisref->BestIndx.p;
	int idM=thisref->idM,
		idm=thisref->idm,
		mx=thisref->X.m,
		nx=thisref->X.n,
		nz=nx+1;
	double dist,tmp,e,b,sse,eC,eB;
	int i,j,k,m,p;
	
	BestDist[0] = 0;
    for (p=1; p<=idM; p++) BestDist[p] = INF;

    if (Wvec)
	{
		for (i=0; i<mx; i++)
		{
			dist = 0.0;
		// Don't break the search
		//for (j=0; j<nx && dist < BestDist[idM] ; j++){
			for (j=0; j<nx; j++)
			{
				C[j][i] = X[j][i]-Q[j];
				dist += Wvec[j]*fabs(C[j][i]);
			}
			for(p=idM; dist < BestDist[p] ; p--)
			{
				BestDist[p+1] = BestDist[p];
				BestIndx[p] = BestIndx[p-1];
			}
			BestDist[p+1] = dist;
			BestIndx[p] = i;
		}
    } else 
	{
		for (i=0; i<mx; i++)
		{
			dist = 0.0;
			// Don't break the search
			//for (j=0; j<nx && dist < BestDist[idM] ; j++){
			for (j=0; j<nx; j++)
			{
				C[j][i] = X[j][i]-Q[j];
				dist += fabs(C[j][i]);
			}
			for(p=idM; dist < BestDist[p] ; p--)
			{
				BestDist[p+1] = BestDist[p];
				BestIndx[p] = BestIndx[p-1];
			}
			BestDist[p+1] = dist;
			BestIndx[p] = i;
		}
    }

    /*    Reinitialize v    */
    for (i=0; i<nz*nz; i++)  Vc[i] = 0.0;
    for (j=0; j<nz; j++)     v[j][j] = LAMBDA;

    Zvec = *Z;
    for(i=0;i<idM;i++)
	{
		k = BestIndx[i];
		W[i] = Y[k];
		*(Zvec++) = 1.0;
		for(j=0;j<nx;j++) *(Zvec++) = C[j][k];
    }

    for(i=0;i<nz;i++) t[i] = 0.0;

    for (k=0; k<idM; k++)
	{
		e = W[k];
		b = 1;
		for (i=0; i<nz; i++)
		{
			tmp=0;
			for(j=0; j<nz; j++)	tmp += v[j][i] * Z[k][j];
			a[i] = tmp;
			b += Z[k][i] * tmp;
			e -= Z[k][i] * t[i];
		}
		for (i=0; i<nz; i++) for(j=0; j<nz; j++) v[j][i] -= a[i] * a[j] / b;
		for (i=0; i<nz; i++)
		{
			tmp=0;
			for(j=0; j<nz; j++) tmp += v[j][i] * Z[k][j];
			t[i] += e * tmp;
		}
	    if (k>0)
		{
			sse=0;
			for(m=0; m<=k; m++)
			{
				e = W[m];
				b = 1;
				for (i=0; i<nz; i++)
				{
					tmp=0;
					for(j=0; j<nz; j++) tmp += v[j][i] * Z[m][j];
					b -= Z[m][i] * tmp;
					e -= Z[m][i] * t[i];
				};
				sse += pow(e/b,2);
			};
			eC = sse/(k+1);
		} else 
		{
			eC=INF;
			eB=eC;
		};

	    if ( (k>=idm-1) && (eC < eB) ) 
		{
			memcpy(tB,t,nz*sizeof(double));
			eB=eC;
		};
    };
};

double lazy_eval(lazy* thisref,vector *in)
{
	lazy_work(thisref,in);
	return thisref->tB.p[0];
};

double lazy_jacobinput(lazy *thisref,int k,vector *in)
{
	lazy_work(thisref,in);
	return thisref->tB.p[1+k];
};

/* =======================linear object===================================*/
linear* linear_Init(linear *_thisref, const mxArray *pmiso,vector nu, vector nd, 
                    vector ny, int nOut)
{
    linear *thisref;
    if (_thisref==NULL)
    {
        thisref=(linear*)mxMalloc(sizeof(linear));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

    syst_Init((syst*)thisref,pmiso,nu,nd,ny,nOut);
    thisref->type='l';
	thisref->linears=getVector(pmiso,"linears");
	thisref->optimParams.n=0;
	return thisref;
};

void linear_Terminate(linear *thisref){};

double linear_eval(linear *thisref,int out,vector *in)
{
	int j,n=thisref->nEntree;
	dataPtr i=in->p,l=thisref->linears.p+(n+1)*out;
	double sum=*(l+n);
	for (j=in->n; j>0; j--)
	{
		sum+=(*i)*(*l); i++; l++;
	}
	return sum;
};

double linear_jacobinput(linear *thisref,int out,int k,vector *in)
{
	return thisref->linears.p[k+out*(thisref->nEntree+1)];
};

/* =======================gaussian object===============================*/

double gaus_MembershipFnct(gaussian *thisref,double d, int i)
{
    if (thisref->type=='h') return hyp_MembershipFnct((hyperbolic*)thisref,d,i);
    return exp(-d);
};

gaussian* gaus_Init(gaussian *_thisref, const mxArray *pmiso,vector nu, vector nd, 
                    vector ny, int nOut)
{
    gaussian *thisref;	
    vector tamp;
	vectorI tampI;
	int nEntree,nRules,a,b,c,i,j;
    if (_thisref==NULL)
    {
        thisref=(gaussian*)mxMalloc(sizeof(gaussian));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

    syst_Init((syst*)thisref,pmiso,nu,nd,ny,nOut);
	nEntree=thisref->nEntree;
    thisref->type='g';
    
	thisref->nRules=(int)GetDouble(pmiso,"n_rules");
	nRules=thisref->nRules;

    thisref->Centers  =getVector(pmiso,"centers"   );
    thisref->Variances=getVector(pmiso,"ivariances");
    thisref->Linears  =getVector(pmiso,"linears"   );

// renumber the "optimparams" matrix to match "our" definition
	j=0;
    thisref->subParams=0;
	for (i=0; i<thisref->optimParams.n; i++)
	{
		a=thisref->optimParams.p[i]-1;
		b=nEntree*nRules;
		if (a<b)
		{ 
			thisref->optimParams.p[j]=a; j++; thisref->subParams|=1;
		}
		else
		{
			if (a<b+SQR(nEntree)*nRules)
			{
				c=0;
				while (a>=b)
				{
					c+=nEntree*(nEntree-1)/2; b+=SQR(nEntree);
				}
				if (a<b-nEntree*(nEntree-1)/2)
				{
					thisref->optimParams.p[j]=a-c; j++;
					thisref->subParams|=2;
				};
			}
			else
			{
				thisref->optimParams.p[j]=a-nRules*nEntree*(nEntree-1)/2; j++;
				thisref->subParams|=4+8;
			};
		};
	};
	vi_Init(&tampI,j);
	memcpy(tampI.p,thisref->optimParams.p,tampI.n*sizeof(int));
	vi_Terminate(&thisref->optimParams);
	thisref->optimParams=tampI;

    if (save==0)
    {
        if  ((thisref->subParams&1)!=0)
        {
            v_Init(&tamp,thisref->Centers.n);
            memcpy(tamp.p,thisref->Centers.p,thisref->Centers.n*sizeof(double));
            thisref->Centers=tamp;
        };
        if  ((thisref->subParams&2)!=0)  
        {
            v_Init(&tamp,thisref->Variances.n);
            memcpy(tamp.p,thisref->Variances.p,thisref->Variances.n*sizeof(double));
            thisref->Variances=tamp;
        };
        if (((thisref->subParams&4)!=0)||
            ((thisref->subParams&8)!=0)  )
        {
            v_Init(&tamp,thisref->Linears.n);
            memcpy(tamp.p,thisref->Linears.p,thisref->Linears.n*sizeof(double));
            thisref->Linears=tamp;
        };
    };


// for the 'eval' and 'jacob???' functions :
    v_Init(&thisref->xMinC,thisref->nRules*thisref->nEntree);
    v_Init(&thisref->y,thisref->nRules);
    v_Init(&thisref->memb,thisref->nRules);

    return thisref;
};

double gaus_getCenter(gaussian *thisref,int rule, int entree)
{
    return thisref->Centers.p[rule*thisref->nEntree+entree];
};

void gaus_setCenter(gaussian *thisref,int rule, int entree, double d)
{
    thisref->Centers.p[rule*thisref->nEntree+entree]=d;
};

double gaus_getVariance(gaussian *thisref,int rule, int i, int j)
{
    return thisref->Variances.p[rule*SQR(thisref->nEntree)+i*thisref->nEntree+j];
};

void gaus_setVariance(gaussian *thisref,int rule, int i, int j, double d)
{
    thisref->Variances.p[rule*SQR(thisref->nEntree)+i*thisref->nEntree+j]=d;
};

double gaus_getLinear(gaussian *thisref,int rule, int entree)
{
    return thisref->Linears.p[rule*(thisref->nEntree+1)+entree];
};

void gaus_setLinear(gaussian *thisref,int rule, int entree, double d)
{
    thisref->Linears.p[rule*(thisref->nEntree+1)+entree]=d;
};

void gaus_Terminate(gaussian *thisref)
{
    if (save==0)
    {
        if  ((thisref->subParams&1)!=0)   v_Terminate(&thisref->Centers);
        if  ((thisref->subParams&2)!=0)   v_Terminate(&thisref->Variances);
        if (((thisref->subParams&4)!=0)||
            ((thisref->subParams&8)!=0))  v_Terminate(&thisref->Linears);
    };

    v_Terminate(&thisref->xMinC);
    v_Terminate(&thisref->memb);
    v_Terminate(&thisref->y);
};

double gaus_eval(gaussian *thisref,vector *in)
                                        /*gaussian membership*/
{
    double out;
    double temp;
    int i,j,r,s;

    int nRules=thisref->nRules;
    int nEntree=thisref->nEntree;
    dataPtr xMinC=thisref->xMinC.p;
    dataPtr memb=thisref->memb.p;
    dataPtr y=thisref->y.p;

    //**/ if (in->n!=nEntree) error("gaussian eval");
    
    /* Compute the differences between x and the centers */
    for (i=0; i<nRules;  i++)
     {
        /* rule i: */ 
         for(j=0;j<nEntree;j++)
         {
             xMinC[i*nEntree+j]=(in->p)[j]-gaus_getCenter(thisref,i,j);
         }
     }
     
     /* Compute the membership functions*/
     for (i=0;i<nRules;i++)
     {
         memb[i] = 0;
         /* Non diagonal elements */ 
         for(r=0;r<nEntree;r++)
         {
            temp=0;
             for(s=r+1;s<nEntree;s++)
             {
                 temp+=gaus_getVariance(thisref,i,r,s)*xMinC[i*nEntree+s];
             }
            memb[i]+=xMinC[i*nEntree+r]*temp;
         }
         memb[i] *= 2;
        
         /* Diagonal elements */        
        for(r=0;r<nEntree;r++)
         {
             memb[i] +=  xMinC[i*nEntree+r]*
                        gaus_getVariance(thisref,i,r,r)*
                        xMinC[i*nEntree+r];
         }
        memb[i]=gaus_MembershipFnct(thisref,memb[i],i);
     }
    
    /* Compute the sum of the memberships */
    thisref->sumMemb = 2.2251e-308;
    for (i=0;i<nRules;i++)
     {    
         thisref->sumMemb += memb[i];
     }
     
     /* Compute the local outputs */
          
     for (i=0;i<nRules;i++)
     {
         y[i] = 0;
         for(r=0;r<nEntree;r++)
         {
            y[i] += (in->p)[r]*gaus_getLinear(thisref,i,r);
         }
         y[i] += gaus_getLinear(thisref,i,nEntree);
     }
     
     /* Compute the global outputs */
    out = 0;
    for (i=0;i<nRules;i++)
     {    
         out += memb[i] * y[i];
    }
     out /= thisref->sumMemb;
    return out;
}

double gaus_jacobinput(gaussian *thisref,int k, vector *in)
// k=sortie 
{
    double sum=0,out;
    int i,nEntree;

    out=gaus_eval(thisref,in); // +initialise memb[],xMinC[],y[], sumMemb 
    nEntree=thisref->nEntree;

    for (i=0; i<thisref->nRules; i++)
     {    
/*      temp=0;
        for (s=0; s<nEntree; s++) 
            temp+=gaus_getVariance(thisref,i,k,s)*thisref->xMinC.p[i*nEntree+s]; */

         sum+= thisref->memb.p[i]*(gaus_getLinear(thisref,i,k));
//             -2*temp*(thisref->y.p[i]-out));
     };
     return sum/thisref->sumMemb;
};

double gaus_jacoblinear(gaussian *thisref,int k,int l,vector *in)
/* k=rule; l=entree */
{
    gaus_eval(thisref,in);
    return thisref->memb.p[k]*((l==thisref->nEntree)?1:(in->p)[l])/thisref->sumMemb;
};

double gaus_jacobvariance(gaussian *thisref,int k,int l, int m, vector *in)
/* k=rule; l,m= indice dans la matrice de variance */
{
    double out;
    int nEntree=thisref->nEntree;
    dataPtr xMinC=thisref->xMinC.p;

    if (thisref->type=='h') 
        return hyp_jacobvariance((hyperbolic*)thisref,k,l,m,in);
    
    out=gaus_eval(thisref,in);
    return -thisref->memb.p[k]*xMinC[k*nEntree+l]*xMinC[k*nEntree+m]/
           thisref->sumMemb*(thisref->y.p[k]-out);
};

double gaus_jacobcenter(gaussian *thisref,int k, int l, vector *in)
/* k=rule; l=centre */
{
    int s;
    double sum=0,out;

    if (thisref->type=='h') return hyp_jacobcenter((hyperbolic*)thisref,k,l,in);

    out=gaus_eval(thisref,in);
    for (s=0; s<thisref->nEntree; s++)
        sum+=gaus_getVariance(thisref,k,l,s)*thisref->xMinC.p[k*thisref->nEntree+s];
     return 2*sum*thisref->memb.p[k]/thisref->sumMemb*(thisref->y.p[k]-out);
};

int    gaus_numParams(gaussian *thisref)
{
    int tamp=0;
    int nRules=thisref->nRules;
    int nEntree=thisref->nEntree;
    int subParams=thisref->subParams;

    if ((subParams&1)!=0) /* centers */
        tamp+=nEntree*nRules;
    if ((subParams&2)!=0) /* variances (matrice symÈtrique) */
        tamp+=nRules*(nEntree*(nEntree+1)/2);
    if ((subParams&4)!=0) /* linears */
        tamp+=nEntree*nRules;
    if ((subParams&8)!=0) /* offsets */
        tamp+=nRules;
    return tamp;
};

double gaus_workParam(gaussian *thisref,int toDo,int p,vector *entree_work,double set)
{
    int a,b,c,d,j;
    int nRules=thisref->nRules;
    int nEntree=thisref->nEntree;
    int subParams=thisref->subParams;
    
    /* centers */
    {
        a=nEntree*nRules;
        if (p<a)
        {
            b=p%nEntree;
            c=p/nEntree;
            switch (toDo)
            {
            case 0:return gaus_jacobcenter(thisref,c,b,entree_work);
            case 1:return gaus_getCenter(thisref,c,b);
            case 2:gaus_setCenter(thisref,c,b,set);
                   return 0;
            }
        }
        p-=a;
    };
    
	/* variances (matrice symÈtrique) */
    {
        a=nRules*(nEntree*(nEntree+1)/2);
        if (p<a) 
        {            
            d=nEntree*(nEntree+1)/2;
            b=p/d;
            p=p%d;
            for (j=0; j<nEntree; j++)
            {
                if (p<=j)
                    switch (toDo)
                    {
                    case 0:return gaus_jacobvariance(thisref,b,j,p,entree_work);
                    case 1:return gaus_getVariance(thisref,b,j,p);
                    case 2:gaus_setVariance(thisref,b,j,p,set);
                           gaus_setVariance(thisref,b,p,j,set);
                           return 0;
                    }
                else p-=j+1;
            }
        }
        p-=a;
    };
    
	/* linears */
    {
            b=p%(nEntree+1);
            c=p/(nEntree+1);
            switch (toDo)
            {
            case 0:return gaus_jacoblinear(thisref,c,b,entree_work);
            case 1:return gaus_getLinear(thisref,c,b);
            case 2:gaus_setLinear(thisref,c,b,set);
                   return 0;
            }
    };
    return 0;
};

double gaus_jacobparam(gaussian *thisref,int p, vector *entree_work)
{
    return gaus_workParam(thisref,0,p,entree_work,0);
};

double gaus_getParam(gaussian *thisref,int p)
{
    return gaus_workParam(thisref,1,p,NULL,0);
};

void gaus_setParam(gaussian *thisref,int p,double d)
{
    gaus_workParam(thisref,2,p,NULL,d);
};

/* =======================hyperbolic object=================================*/

void hyp_Terminate(hyperbolic *thisref)
{
    v_Terminate(&thisref->jacobTmp);
	gaus_Terminate((gaussian*)thisref);
};

hyperbolic *hyp_Init(hyperbolic *_thisref,const mxArray *pmiso,
    vector nu, vector nd, vector ny, int nOut)
{
    hyperbolic *thisref;
    if (_thisref==NULL) 
    {
        thisref=(hyperbolic*)mxMalloc(sizeof(hyperbolic));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

    gaus_Init((gaussian*)thisref,pmiso,nu,nd,ny,nOut);
    thisref->type='h';
    thisref->m=GetDouble(pmiso,"m");
    v_Init(&thisref->jacobTmp,thisref->nRules);
    return thisref;
};

double hyp_jacobinput(hyperbolic *thisref,int k, vector *in)
// k=sortie 
{
    double sum=0,out;
    int i;
    int nEntree=thisref->nEntree;
    out=gaus_eval((gaussian*)thisref,in); //+initialise memb[],jacobTmp[],xMinC[],y[],sumMemb 
    for (i=0; i<thisref->nRules; i++)
     {    
/*      temp=0;
        for (s=0; s<nEntree; s++) 
            temp+=gaus_getVariance((gaussian*)thisref,i,k,s)*thisref->xMinC.p[i*nEntree+s];*/
         sum+= thisref->memb.p[i]*gaus_getLinear((gaussian*)thisref,i,k); 
//              +thisref->jacobTmp.p[i]*2*temp*(thisref->y.p[i]-out);
     };
     return sum/thisref->sumMemb;
};

double hyp_jacobvariance(hyperbolic *thisref,int k,int l, int m, vector *in)
/* k=rule; l,m= indice dans la matrice de variance */
{
    int nEntree=thisref->nEntree;
    dataPtr xMinC=thisref->xMinC.p;
    double out=gaus_eval((gaussian*)thisref,in);
    return thisref->jacobTmp.p[k]*xMinC[k*nEntree+l]*xMinC[k*nEntree+m]/
           thisref->sumMemb*(thisref->y.p[k]-out);
};

double hyp_jacobcenter(hyperbolic *thisref,int k, int l, vector *in)
/* k=rule; l=centre */
{
    int s;
    double sum=0,out;
    int nEntree=thisref->nEntree;

    out=gaus_eval((gaussian*)thisref,in);
    for (s=0; s<nEntree; s++)
        sum+=gaus_getVariance((gaussian*)thisref,k,l,s)*thisref->xMinC.p[k*nEntree+s];
    return -2*sum*thisref->jacobTmp.p[k]/thisref->sumMemb*(thisref->y.p[k]-out);
};

double hyp_MembershipFnct(hyperbolic *thisref,double d,int i)
{
    thisref->jacobTmp.p[i]=pow(d+1e-100,-1/(thisref->m-1)-1)*-1/(thisref->m-1);
    return pow(d+1e-100,-1/(thisref->m-1));
};

/* =======================filter object===========================*/

filterT * fil_Init(filterT *_thisref,const mxArray *params,StateMatrix *_X)
{
	filterT *thisref;
    if (_thisref==NULL) 
    {
        thisref=(filterT*)mxMalloc(sizeof(filterT));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;
	thisref->num=getVector(params,"numerator");
	thisref->den=getVector(params,"denominator");
	thisref->memory=thisref->num.n/plant->nOut;
	thisref->X=_X;
	thisref->initial=thisref->memory+1;
	v_Init(&thisref->savePred,thisref->num.n);
	v_Init(&thisref->saveF,thisref->den.n);
	memset(thisref->savePred.p,0,thisref->savePred.n*sizeof(double));
	memset(thisref->saveF.p,0,thisref->saveF.n*sizeof(double));
	return thisref;
};

void fil_Terminate(filterT *thisref)
{
    v_Terminate(&thisref->savePred);
	v_Terminate(&thisref->saveF);	
};

void fil_initFromMatlab(filterT *thisref,const double *uc)
{
	int i,j,tmp,tmp2;
	double sum;
	double *p=thisref->savePred.p,
		   *pf=thisref->saveF.p,
		   *NUM=thisref->num.p,*DEN=thisref->den.p;
	StateMatrix *X=thisref->X;

	if (thisref->initial) thisref->initial--;
	memmove(p+plant->nOut, p, (thisref->savePred.n-plant->nOut)*sizeof(double));
	memmove(pf+plant->nOut, pf, (thisref->saveF.n-plant->nOut)*sizeof(double));
	for (j=0; j<plant->nOut; j++)
	{		
		p[j]=uc[j+X->setPoints]-st_Get(X,1,j,OUT,PLANT)+pf[j]; 
		pf[j]=0;
		sum=0;
		for (i=0; i<thisref->memory; i++)
		{
			tmp=i*plant->nOut+j;
			sum+=p[tmp]*NUM[tmp]-pf[tmp]*DEN[tmp];
		};
		pf[j]=sum/DEN[j];
	};
};

/* =======================StateMatrix object===========================*/

StateMatrix * st_Init(StateMatrix *_thisref,const mxArray *mx,int *setPoints,int _horizon)
{
	const mxArray *pa;
	dataPtr tmp;
    int i,j,a=0;
    StateMatrix *thisref;
    if (_thisref==NULL) 
    {
        thisref=(StateMatrix*)mxMalloc(sizeof(StateMatrix));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

	pa=mxGetField(mx,0,"setpoints");
    if (pa==NULL)
    {    
		thisref->setPoints=cntrl->nOut;
    } else	
	{
		a=0;
		j=mxGetNumberOfElements(pa);
		tmp=(dataPtr)mxGetPr(pa);
		for (i=0; i<j; i++) if (tmp[i]!=i+1) a=1;
		if (a==1) error("the setpoints matrix must be of the form [1:n]");
		thisref->setPoints=j;
	}
	*setPoints=thisref->setPoints;

	thisref->sampTime=GetDouble(ssGetSFcnParam(S,0),"sampTime");
    thisref->horizon=_horizon;
	thisref->CTime=-3;

    for (j=0; j<cntrl->nOut; j++)
	{
        for (i=0; i<mimo_get(cntrl,j)->nu.n; i++)
            a=MAX(mimo_get(cntrl,j)->nu.p[i]+mimo_get(cntrl,j)->nd.p[i],a);
        for (i=0; i<mimo_get(cntrl,j)->ny.n; i++)
            a=MAX(mimo_get(cntrl,j)->ny.p[i]+1,a);
	}
    for (j=0; j<plant->nOut; j++)
	{
        for (i=0; i<mimo_get(plant,j)->nu.n; i++)
            a=MAX(mimo_get(plant,j)->nu.p[i]+mimo_get(plant,j)->nd.p[i]-1,a);
		for (i=0; i<mimo_get(plant,j)->ny.n; i++)
			a=MAX(mimo_get(plant,j)->ny.p[i],a);
	}	
    thisref->max_time=a;

    thisref->xsize=(cntrl->nIn+cntrl->nOut)*(thisref->max_time)*sizeof(double); //le x est celui de matlab
    v_Init(&thisref->v,(cntrl->nIn+cntrl->nOut)*(thisref->max_time+_horizon+1)); //initialisation de mon x qui est + grand		
	memset(thisref->v.p,0,thisref->v.n*sizeof(double));

    for (i=0; i<thisref->setPoints; i++)
        st_Set(thisref,0,i,SP,0);
    return thisref;
};

void st_Terminate(StateMatrix *thisref)
{
    v_Terminate(&thisref->v);
};

void st_CreateEntree(StateMatrix *thisref, mimo* pmimo, int nsyst,int k, vector *result)
{
    int i=0,j,l,shift=0;
	syst *f=mimo_get(pmimo,nsyst);
    if (pmimo->type==CONTROLLER) shift=-1;
    for (j=0; j<f->ny.n; j++)
        for (l=0; l<f->ny.p[j]; l++)
        {
            result->p[i]=st_Get(thisref,k-l+shift,j,OUT,pmimo->type);
            i++;
        };
	shift=1;
	if (pmimo->type==CONTROLLER) shift=0;
    for (j=0; j<f->nu.n; j++)
        for (l=0; l<f->nu.p[j]; l++)
        {
            result->p[i]=st_Get(thisref,k-l-(f->nd.p[j])+shift,j,IN,pmimo->type);
            i++;
        };
    result->n=i;
};

double st_Get(StateMatrix *thisref,int k, int i, int kind, int forWhat)
{
    int base=(k+thisref->max_time)*(cntrl->nIn+cntrl->nOut);

	if (base<0) error("statematrix access error");
    if (kind==SP) return thisref->v.p[base+i+cntrl->nOut];
    if (forWhat==CONTROLLER)
         if (kind==OUT)
            if (i<cntrl->nOut) return thisref->v.p[base+i];
            else return thisref->v.p[base+thisref->setPoints+plant->nOut+i];
         else return thisref->v.p[base+i+cntrl->nOut];
    else if (kind==IN)
            if (i<cntrl->nOut) return thisref->v.p[base+i];
            else return thisref->v.p[base+thisref->setPoints+plant->nOut+i];
         else return thisref->v.p[base+thisref->setPoints+i+cntrl->nOut];
};

void st_Set(StateMatrix *thisref,int k, int i, int kind, double value)
{
   double tamp;
   //**/ if (k>=horizon) error("sk"); 
   switch (kind)
   {
   case US://**/ if (i>=cntrl->nOut) error("su");
           tamp=plant->minmax.p[i*2];
           if (value<tamp) value=tamp;
           tamp=plant->minmax.p[i*2+1];
           if (value>tamp) value=tamp;

           tamp=cntrl->minmax.p[(i+cntrl->nIn)*2];
           if (value<tamp) value=tamp;
           tamp=cntrl->minmax.p[(i+cntrl->nIn)*2+1];
           if (value>tamp) value=tamp;

           thisref->v.p[(k+thisref->max_time)*(cntrl->nIn+cntrl->nOut)+i]=value;
           break;
   case YS://**/ if (i>=plant->nOut) error("sy");
           tamp=cntrl->minmax.p[(i+thisref->setPoints)*2];
           if (value<tamp) value=tamp;
           tamp=cntrl->minmax.p[(i+thisref->setPoints)*2+1];
           if (value>tamp) value=tamp;

           tamp=plant->minmax.p[(i+plant->nIn)*2];
           if (value<tamp) value=tamp;
           tamp=plant->minmax.p[(i+plant->nIn)*2+1];
           if (value>tamp) value=tamp;

           thisref->v.p[(k+thisref->max_time)*(cntrl->nIn+cntrl->nOut)+i+
                        cntrl->nOut+thisref->setPoints]=value;
           break;
   case SP://**/ if (i>=setPoints) error("ssp");
           thisref->v.p[(k+thisref->max_time)*(cntrl->nIn+cntrl->nOut)+cntrl->nOut+i]=value;
           break;
   }
};

void st_InitFromMatlab(StateMatrix *thisref,const double *x,const double *Y,time_T _t)
{
    int k,i,j,a;

	thisref->CTime=_t;
    memcpy(thisref->v.p,x,thisref->xsize);
	a=thisref->max_time*(cntrl->nIn+cntrl->nOut)+cntrl->nOut;
    for (i=0; i<cntrl->nIn; i++)
        thisref->v.p[a+i]=*(Y+i);
                //new controller inputs for SP,YS,Non-Controllable_Inputs

    for (k=0; k<thisref->horizon-1; k++)
    {
        for (i=0; i<thisref->setPoints; i++)
            st_Set(thisref,k+1,i,SP,st_Get(thisref,k,i,SP,PLANT));    // setpoints
        for (i=thisref->setPoints+plant->nOut; i<cntrl->nIn; i++)
        {
            j=(k+thisref->max_time)*(cntrl->nIn+cntrl->nOut)+i+cntrl->nOut;
            thisref->v.p[j+(cntrl->nIn+cntrl->nOut)]=thisref->v.p[j]; // non-controllable_inputs
        }
    }
};

void st_UpdateMatlab(StateMatrix *thisref,double *x)
{
    memcpy(x,(thisref->v.p+cntrl->nIn+cntrl->nOut),thisref->xsize);
}

/* =======================Deriv Matrix object==========================*/

DerivMatrix *der_Init(DerivMatrix *_thisref,int _spd, int _horizon,int numTParams)
{
    int i,p;
    DerivMatrix *thisref;
    if (_thisref==NULL)
    {
        thisref=(DerivMatrix*)mxMalloc(sizeof(DerivMatrix));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

    thisref->horizon=_horizon;
    thisref->shift=(cntrl->nOut+plant->nOut)*(_horizon);
    v_Init(&thisref->v,thisref->shift*numTParams);
	memset(thisref->v.p,0,thisref->v.n*sizeof(double));
    thisref->setPoints=_spd;
    for (p=0; p<numTParams; p++)
        for (i=0; i<plant->nOut; i++) der_Set(thisref,p,0,i,YS,0);
    return thisref;
}

void der_Terminate(DerivMatrix *thisref)
{
    v_Terminate(&thisref->v);
}

double der_Get(DerivMatrix *thisref,int p, int time, int indice, int kind,int forWhat)
{
    if (time<0) return 0;

    if (forWhat==CONTROLLER)
    {
        if (kind==OUT)
        {
            if (indice<cntrl->nOut) 
                return thisref->v.p[p*thisref->shift+time*(cntrl->nOut+plant->nOut)+indice];
            return 0;
        }
        if ((indice>=thisref->setPoints)&&(indice<plant->nOut+thisref->setPoints))
            return thisref->v.p[p*thisref->shift+time*(cntrl->nOut+plant->nOut)+cntrl->nOut+indice-thisref->setPoints];
        return 0;
    }
    if (kind==IN)
    {
        if (indice<cntrl->nOut) 
            return thisref->v.p[p*thisref->shift+time*(cntrl->nOut+plant->nOut)+indice];
        return 0;
    }
    if (indice<plant->nOut)
        return thisref->v.p[p*thisref->shift+time*(cntrl->nOut+plant->nOut)+cntrl->nOut+indice];
    return 0;

};

void der_Set(DerivMatrix *thisref,int p,int time, int indice, int kind, double d)
{
    //**/ if (time>horizon) error("dk");
    switch (kind)
    {
    case US: if (indice>=cntrl->nOut) error("du");
            thisref->v.p[p*thisref->shift+time*(cntrl->nOut+plant->nOut)+indice]=d;
            break;
    case YS: if (indice>=plant->nOut) error("dy");
            thisref->v.p[p*thisref->shift+time*(cntrl->nOut+plant->nOut)+cntrl->nOut+indice]=d;
            break;
    }
};

double der_Get2(DerivMatrix *thisref,int p,int time,int indice,int kind)
{
    if (time<0) return 0;
    return thisref->v.p[p*thisref->shift+time*(plant->nOut+cntrl->nOut)+
                   indice+kind*cntrl->nOut]; //kind=  (US=0) or (YS=1)
};

/* ==========================debug object===============================*/

debugClass *db_Init(debugClass *_thisref,int toDo,const mxArray *mx,StateMatrix *_X,DerivMatrix *_D)
{
    int l,j;
    gaussian *g;
    debugClass *thisref;
    if (_thisref==NULL)
    {
        thisref=(debugClass*)mxMalloc(sizeof(debugClass));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

    thisref->sampTime=GetDouble(ssGetSFcnParam(S,0),"sampTime");
    l=(int)((ssGetTFinal(S)-ssGetTStart(S))/thisref->sampTime+1);
    thisref->dbug=(int)GetDoublePlus(mx,"debug",0);
    thisref->toDo=toDo;

    if (thisref->dbug==1)
    {
        thisref->X=_X; thisref->deriv=_D;

        v_Init(&thisref->u,l*cntrl->nOut);
        v_Init(&thisref->y,l*plant->nOut);
        v_Init(&thisref->t,l);
        v_Init(&thisref->sp,l*_X->setPoints);
        thisref->li_largeur=0; thisref->memb_largeur=0;
        for (j=0; j<cntrl->nOut; j++)
        {
            g=(gaussian*)mimo_get(cntrl,j);
            thisref->li_largeur+=g->nRules*(1+g->nEntree);
            thisref->memb_largeur+=g->nRules;
        };
        v_Init(&thisref->li,l*thisref->li_largeur);
        v_Init(&thisref->memb,l*thisref->memb_largeur);
		v_Init(&thisref->x,l*thisref->X->v.n);
		v_Init(&thisref->d,l*thisref->deriv->v.n);
        if ((toDo&1)!=0)
            v_Init(&thisref->etha,l);
    };
    return thisref;
};

void db_saveEtha(debugClass *thisref, double etha)
{
    int i=(int)floor(ssGetT(S)/thisref->sampTime);
    if ((thisref->dbug==1)&&(i>=0))
        thisref->etha.p[i]=etha;
};

void db_saveALL(debugClass *thisref)
{
   long int shift,j,k,l,i=(int)floor(ssGetT(S)/thisref->sampTime);
    gaussian *g;
    if ((thisref->dbug==1)&&(i>=0))
    {
        shift=i*cntrl->nOut;
        for (j=0; j<cntrl->nOut; j++)
            thisref->u.p [shift+j]=st_Get(thisref->X,0,j,OUT,CONTROLLER);
        shift=i*plant->nOut;
        for (j=0; j<plant->nOut; j++)
            thisref->y.p [shift+j]=st_Get(thisref->X,0,j,OUT,PLANT);
        thisref->t.p [i]=ssGetT(S);
        shift=i*thisref->X->setPoints;
        for (j=0; j<thisref->X->setPoints; j++)
            thisref->sp.p[shift+j]=st_Get(thisref->X,0,j,SP,PLANT);
        shift=thisref->li_largeur*i; l=0;
        for (j=0; j<cntrl->nOut; j++)
        {
            g=(gaussian*)mimo_get(cntrl,j);
            k=g->nRules*(1+g->nEntree);
            memcpy(thisref->li.p+shift+l, g->Linears.p,k*sizeof(double));
            l+=k;
        }
        shift=thisref->memb_largeur*i; l=0;
        for (j=0; j<cntrl->nOut; j++)
        {
            g=(gaussian*)mimo_get(cntrl,j);
            k=g->nRules;
            memcpy(thisref->memb.p+shift+l,g->memb.p,k*sizeof(double));
            l+=k;
        };
        shift=thisref->deriv->v.n;
        memcpy(thisref->d.p+shift*i,thisref->deriv->v.p,shift*sizeof(double));
		shift=thisref->X->v.n-cntrl->nIn-cntrl->nOut; 
        memcpy(thisref->x.p+shift*i,thisref->X->v.p,shift*sizeof(double));
    };
};

void db_saveALLagain(debugClass *thisref)
{
    int i=(int)floor(ssGetT(S)/thisref->sampTime);
    if ((thisref->dbug==1)&&(i>=0))
    {
        db_saveALL(thisref);
        if (((thisref->toDo&1)!=0)&&(i>0)) 
            thisref->etha.p[i]=thisref->etha.p[i-1];
    };
};

void db_sendToMatlab0(double d,mxArray *base,const char *name)
{
    int i;
    mxArray *array_ptr= mxCreateDoubleMatrix(1,1,mxREAL);
    mxArray *old_field=mxGetField(base, 0, name);
    double *pr;

    if (old_field) mxDestroyArray(old_field);
    pr=(double*)mxGetPr(array_ptr);
    mexPutArray(array_ptr,"base");
    *pr=d;
    mxSetField(base,0, name, array_ptr);    
};

void db_sendToMatlab2(int l,vector *v,int n,mxArray *base,const char *name)
{
    int i,j;
    mxArray *array_ptr= mxCreateDoubleMatrix(l,n,mxREAL);
    mxArray *old_field=mxGetField(base, 0, name);
    double *pr;

    if (old_field) mxDestroyArray(old_field);
    pr=(double*)mxGetPr(array_ptr);
    mexPutArray(array_ptr,"base");

    for (i=0; i<n; i++)
        for (j=0; j<l; j++)    
            pr[i*l+j]=v->p[n*j+i];
    
    mxSetField(base,0, name, array_ptr);
    if (v->n!=0) v_Terminate(v);
};

void db_sendToMatlab3(int l,vector *v,int n,int m,mxArray *base,const char *name)
{
    int i,j;
	int dim[3];
    mxArray *array_ptr,*old_field;    
    double *pr;

	old_field=mxGetField(base, 0, name);
	if (old_field) mxDestroyArray(old_field);    
	dim[0]=l; dim[1]=n; dim[2]=m;
	array_ptr=mxCreateNumericArray(3,(int*)&dim,mxDOUBLE_CLASS,mxREAL);
    pr=mxGetPr(array_ptr);
    mexPutArray(array_ptr,"base");

    for (i=0; i<n*m; i++)
        for (j=0; j<l; j++)
            pr[i*l+j]=v->p[n*m*j+i];

    mxSetField(base,0, name, array_ptr);
    if (v->n!=0) v_Terminate(v);
};

void db_sendToMatlab4(int l,vector *v,int n,int m,int o,mxArray *base,const char *name)
{
    int i,j;
	int dim[4];
    mxArray *array_ptr,*old_field;    
    double *pr;

	old_field=mxGetField(base, 0, name);
	if (old_field) mxDestroyArray(old_field);    
	dim[0]=l; dim[1]=n; dim[2]=m; dim[3]=o;
	array_ptr=mxCreateNumericArray(4,(int*)&dim,mxDOUBLE_CLASS,mxREAL);
    pr=mxGetPr(array_ptr);
    mexPutArray(array_ptr,"base");

    for (i=0; i<n*m*o; i++)
        for (j=0; j<l; j++)
            pr[i*l+j]=v->p[n*m*o*j+i];

    mxSetField(base,0, name, array_ptr);
    if (v->n!=0) v_Terminate(v);
};

void db_Terminate(debugClass *thisref)
{
    int l=(int)floor(ssGetT(S)/thisref->sampTime)+1;
    const int numField=10;
    const char *fieldnames[] = {"toDo","u","y","t","setpoints","linears",
                                "memb","deriv","x","etha"};
    mxArray  *b;
    
    if (thisref->dbug==1)
    {
        b = mxCreateStructMatrix(1,1,numField,fieldnames);
        mxSetName(b, "debug");

		db_sendToMatlab0((double)thisref->toDo,b,fieldnames[0]);		
        db_sendToMatlab2(l,&thisref->u,cntrl->nOut,b,fieldnames[1]);
        db_sendToMatlab2(l,&thisref->y,plant->nOut,b,fieldnames[2]);
        db_sendToMatlab2(l,&thisref->t,1,b,fieldnames[3]);
        db_sendToMatlab2(l,&thisref->sp,thisref->X->setPoints,b,fieldnames[4]);
        db_sendToMatlab2(l,&thisref->li,thisref->li_largeur,b,fieldnames[5]);
        db_sendToMatlab2(l,&thisref->memb,thisref->memb_largeur,b,fieldnames[6]);
		db_sendToMatlab4(l,&thisref->d,cntrl->nOut+plant->nOut,thisref->deriv->horizon,
							cntrl->numTParams,b,fieldnames[7]);
		db_sendToMatlab3(l,&thisref->x,cntrl->nIn+cntrl->nOut,
							thisref->X->max_time+thisref->X->horizon,b,fieldnames[8]);
        if ((thisref->toDo&1)!=0)
            db_sendToMatlab2(l,&thisref->etha,1,b,fieldnames[9]);
        mexPutArray(b,"base");
    };
};

