
// $ Revision: 1.2 $ 
//
// ICclasses.C 
//
// This file is part from the Mex-Files 'simulate' and 'ICctrl'.
//
// To build 'ICctrl' under LINUX systems, type:
//    mex -O ICmain.c ICclasses.c -o ICctrl.mexlx
// To build 'simulate' under LINUX systems, type:
//    mex -O simulation_error.c ICclasses.c -o simulate.mexlx
//
// Copyright (c) by Frank Vanden Berghen.
// All Rights Reserved.

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

#include "mex.h"
#include "ICclasses.h"

extern mimo *cntrl;
extern mimo *plant;
extern mxArray *FcnParam;
extern int memory_type;
/*==================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);
    mexErrMsgTxt(msg);
    return;
}

void *MyMalloc(unsigned int n)
{
    static char msg[256];   //ILLEGAL: to fix use "static char msg[256];"
    void *p;
    if (memory_type==4) p=mxMalloc(n);
    else p=malloc(n);
    if (p!=NULL) return p;
    sprintf(msg,"no memory:%i bytes asked\n",n);
    mexErrMsgTxt(msg);
};

void *MyRealloc(void *t,unsigned int n)
{
    static char msg[256];   //ILLEGAL: to fix use "static char msg[256];"
    void *p;
    if (memory_type==4) p=mxRealloc(t,n);
    else p=realloc(t,n);
    sprintf(msg,"no memory:%i bytes asked\n",n);
    mexErrMsgTxt(msg);
};

void MyFree(void *t)
{
    if (memory_type!=4) free(t);
//    else mxFree(t);
};

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;
};

vector getVectorPlus(const mxArray *mx,char *subindex)
{
    vector v;
    const mxArray *pa;
    pa=mxGetField(mx,0,subindex);
    if (pa==NULL)
    {    
        v.p=NULL; return v;
    };
    v.p=(dataPtr)mxGetPr(pa);
    v.n=mxGetNumberOfElements(pa);
	v.extend=0;
    return v;
};

vector getVectorTest(const mxArray *mx,char *subindex,int _c,int _l)
{
    vector v;
	int c,l;
    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);
    }    
	c=mxGetN(pa); l=mxGetM(pa);
    if ((_c!=0)&&(c!=_c))
    {
        sprintf((char*)&buf,"variable '%s.%s' must have %i columns",
                 mxGetName(mx),subindex,_c);
        error(buf);
    };
    if ((_l!=0)&&(l!=_l))
    {
        sprintf((char*)&buf,"variable '%s.%s' must have %i lines",
                 mxGetName(mx),subindex,_l);
        error(buf);
    };
    v.n=c*l;
    v.p=(dataPtr)mxGetPr(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 "MyFree(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*)MyMalloc(X.n*sizeof(double*));    
    for (i=0; i<X.n; i++, Xvec.p+=m) X.p[i]=Xvec.p;
    return X;
};


/*======================cache object ===================================*/
cache *ca_Init(cache *_thisref,int nIn,int nOut)
{
    cache *thisref;
    if (_thisref==NULL)
    {
        thisref=(cache*)MyMalloc(sizeof(cache));
    }
    else thisref=_thisref;
    v_Init(&thisref->in,nIn);
    v_Init(&thisref->out,nOut);

    thisref->in.n*=sizeof(double);
    thisref->out.n*=sizeof(double);
    return thisref;
};

void ca_Terminate(cache *thisref)
{
    v_Terminate(&thisref->in);
    v_Terminate(&thisref->out);
};

int ca_test(cache *thisref, double *in, double *out)
{
    if (memcmp(thisref->in.p,in,thisref->in.n)==0)
    {
        if (out!=NULL) memcpy(out,thisref->out.p,thisref->out.n);
        return 1;
    }
    return 0;
};

void ca_update(cache *thisref,double *in, double *out)
{
    memcpy(thisref->in.p,in,thisref->in.n);
    memcpy(thisref->out.p,out,thisref->out.n);
};

/* =======================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*)MyMalloc(sizeof(table));        
    }
    else thisref=_thisref;
    thisref->n=_n; thisref->m=_m; thisref->extend=0;
    t=(dataPtr)MyMalloc(_n*_m*sizeof(double));    
    thisref->p=(dataPtr*)MyMalloc(_n*sizeof(double*));    
    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,tmp4;
    if (thisref->extend==0)
    {    
        tmp=(dataPtr)MyMalloc((n*(m+T_EXTEND))*sizeof(double));
        if (tmp==NULL) error("memory allocation error");
        tmp3=thisref->p; tmp4=*tmp3; tmp2=*tmp3+(n-1)*m;
        for (i=0; i<n; i++, tmp+=(m+T_EXTEND)) tmp3[i]=tmp;
        for (i=n-1; i>0; i--)
        {
            memmove(tmp3[i],tmp2,m*sizeof(double));
            tmp2-=m;
        };
        MyFree(tmp4);
        thisref->extend=T_EXTEND;
    } 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=tmp=*thisref->p;
        for (i=0; i<n; i++, tmp2+=(m+thisref->extend), tmp+=m) 
            memmove(tmp,tmp2,m*sizeof(double));
        tmp=(dataPtr)MyRealloc(*thisref->p,n*m*sizeof(double));
        for (i=0; i<n; i++, tmp+=m) thisref->p[i]=tmp;
        thisref->extend=0;
    };
};

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

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

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

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

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

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

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

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

void vi_Terminate(vectorI *thisref)
{
    MyFree(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];
        if (tamp!=NULL)
        {
            syst_Terminate(tamp);
            MyFree(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,nuo,nd,ndo,ny,nyo;
    char str[80],buf[100];
    int sortie,i;
    mimo *thisref;
    if (_thisref==NULL) 
    {
        thisref=(mimo*)MyMalloc(sizeof(mimo));        
    }
    else thisref=_thisref;

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

    thisref->head=(syst *((*)[1]))MyMalloc(nMapping*sizeof(syst*));

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

    nuo=getVector(dyn,"nu");     nu.n=thisref->nIn;
    ndo=getVector(dyn,"nd");     nd.n=thisref->nIn;
    nyo=getVector(dyn,"ny");     ny.n=thisref->nOut;
        
    sortie=0;
    while (sortie<nMapping)
    {
        for (i=0; i<thisref->nOut; i++) if (thisref->links.p[i<<1]==sortie) break;
        if (i==thisref->nOut) {sortie++; continue;}
        nu.p=nuo.p+i; nd.p=ndo.p+i; ny.p=nyo.p+i;
        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);

            if (strncmp((char*)&str,"gaussian",8)==0)
                (*thisref->head)[sortie]=(syst*)gaus_Init(NULL,tmp,nu,nd,ny);

            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);
            else
            {
                tmp2=mxGetField(tmp,0,"examples_x");
                if (tmp2!=NULL) (*thisref->head)[sortie]=(syst*)lazy_Init(NULL,tmp,nu,nd,ny);
                else
                {
                    tmp2=mxGetField(tmp,0,"lookups");
                    if (tmp2!=NULL) (*thisref->head)[sortie]=(syst*)lookup_Init(NULL,tmp,nu,nd,ny);
                    else
                    {
                        if (((*thisref->head)[sortie]=(syst*)defaultMap_Init(NULL,tmp,nu,nd,ny))==NULL)
                        {
                            sprintf((char*)&buf,"unknown mapping %i",sortie+1);
                            error(buf);
                        };
                    };
                };
            };
        };
        sortie++;
    };
    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)
{
    syst *thisref;
    const mxArray *pm=mxGetField(pmiso,0,"mapping");
    vector tmp;
    int i;
    
    if (_thisref==NULL)
    {
        thisref=(syst*)MyMalloc(sizeof(syst));        
    }
    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*ny.n];
    vi_Init(&thisref->nd,nd.n); 
    for (i=0; i<nd.n; i++) thisref->nd.p[i]=(int)nd.p[i*ny.n];
    vi_Init(&thisref->ny,ny.n); 
    for (i=0; i<ny.n; i++) thisref->ny.p[i]=(int)ny.p[i*ny.n];
    
    tmp=getVector(pm,"optimparams"); 
    if (tmp.n!=0) vi_Init(&thisref->optimParams,tmp.n);
    else thisref->optimParams.n=0;
    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];

    thisref->nSortie=(int)GetDouble(pm,"n_out");
    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)
{
    defaultMap *thisref;
    mxArray *in,*nsyst,*plhs[1],*pm=mxGetField(pmiso,0,"mapping");
    double *p,*pnsyst;
    int t,i;
    if (_thisref==NULL)
    {
        thisref=(defaultMap*)MyMalloc(sizeof(defaultMap));        
    }
    else thisref=_thisref;
    syst_Init((syst*)thisref,pmiso,nu,nd,ny);
    thisref->type='d';
    thisref->optimParams.n=0;
    
    thisref->CA=(cache*) MyMalloc(thisref->nSortie*sizeof(cache));    
    for (i=0; i<thisref->nSortie; i++)
        ca_Init(thisref->CA+i,thisref->nEntree,thisref->nEntree+1);

    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);
            // this is the normal exit:
            return thisref;
        };
    };
    mexSetTrapFlag(0);
    if (_thisref==NULL) MyFree(thisref);
    syst_Terminate((syst*)&thisref);
    return NULL;
};

void defaultMap_Terminate(defaultMap *thisref)
{
    int i;
    for (i=0; i<thisref->nSortie; i++) ca_Terminate(thisref->CA+i);
    MyFree(thisref->CA);
    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,*pnsyst,out;

    if (ca_test(thisref->CA+nsyst,in->p,NULL)) return thisref->CA[nsyst].out.p[k];

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

/* =======================lookup object===================================*/
lookup *lookup_Init(lookup *_thisref, const mxArray *pmiso,vector nu, vector nd, 
                    vector ny)
{
    lookup *thisref;
    int nIn,nVertex,i,j;
    const mxArray *tmp=mxGetField(pmiso,0,"scales");
    mxArray *tmp2,*tmp3=NULL;
    if (_thisref==NULL)
    {
        thisref=(lookup*)MyMalloc(sizeof(lookup));        
    }
    else thisref=_thisref;
    syst_Init((syst*)thisref,pmiso,nu,nd,ny);
    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*)MyMalloc(nIn*sizeof(double*));    
    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**) MyMalloc(nVertex*sizeof(double*));                                 

    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*)MyMalloc(nIn*sizeof(double*));
        
        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*)MyMalloc(nIn*sizeof(double*));
        
        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)
{
    MyFree(thisref->scales.p);
    MyFree(thisref->idxs);
    mxDestroyArray(thisref->Y_IN);
    mxDestroyArray(thisref->Y_OUT);
    if (thisref->Dlup.p!=NULL)
    {
        MyFree(thisref->Dlup.p);
        MyFree(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)
{
    double INF=mxGetInf();
    const mxArray *pa;
    vector range,Xvec;
    int idm,idM,nz,mx,i;    
    lazy *thisref;    
    if (_thisref==NULL)
    {
        thisref=(lazy*)MyMalloc(sizeof(lazy));        
    }
    else thisref=_thisref;
    syst_Init((syst*)thisref,pmiso,nu,nd,ny);
    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;
    thisref->cmb=GetDoublePlus(pmiso,"cmb_par",1);

    idm = (int)range.p[1];
    idM = (int)range.p[4];
    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);
    t_Init(&thisref->BestPred,nz,thisref->cmb);
    v_Init(&thisref->BestError,thisref->cmb+1);
    v_Init(&thisref->t_hat,nz);
    v_Init(&thisref->W,idM);
    v_Init(&thisref->t,nz);
    v_Init(&thisref->a,nz);
    v_Init(&thisref->BestDist,idM+2);
    vi_Init(&thisref->BestIndx,idM+1);
    ca_Init(&thisref->CA,thisref->nEntree,nz);

    thisref->BestError.p[thisref->cmb]=INF;
    return thisref;
};

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

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,
            *bestPred=thisref->BestPred.p;
    dataPtr bestError=thisref->BestError.p,
            Wvec=thisref->Wvec.p,
            W=thisref->W.p,
            t=thisref->t.p,
            tB=*bestPred,
            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,
        cmb=thisref->cmb;
    double dist,tmp,e,b,sse,eC,w;
    int i,j,k,m,p;

    if (ca_test(&thisref->CA,in->p,NULL)) return;

    *bestError=INF;
    *BestDist= 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);
       

            if (k>=idm-1)
            {
            i=0;
                while (eC>bestError[i]) i++;
            if (i!=cmb)
            {
                    if (i!=cmb-1)
                    {
                        memmove(bestPred[i+1],bestPred[i],(cmb-i-1)*nz*sizeof(double));
                    memmove(bestError+i+1,bestError+i,(cmb-i-1)*sizeof(double));
                    };
                    memcpy(bestPred[i],t,nz*sizeof(double));
                    bestError[i]=eC;
                };
            };
        };
    };

    e = *bestError;
    e = (e==0)? 1E-20 : e;
    for ( j=0; j<nz; j++) tB[j]/=e;
    w=1/e;

    for(i=1; i<cmb; i++)
    {
       e = bestError[i];
       e = (e==0)? 1E-20 : e;
       for ( j=0; j<nz; j++)
      tB[j] += bestPred[i][j]/e;
       w+=1/e;
    };

    for (j=0; j<nz; j++) tB[j]/=w;

    ca_update(&thisref->CA,in->p,tB);
};

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

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

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

    syst_Init((syst*)thisref,pmiso,nu,nd,ny);
    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);
};

char params_convert(int *i, int nRules, int nEntree)
{
    int a=*i-1,b=nEntree*nRules,c,d,e,f;

    if (a<b)
    {
        *i=a;
        return 1;
    };  

    a-=b;
    c=SQR(nEntree)*nRules;
    if (a<c)
    {        
        c=a%SQR(nEntree);
        d=c%nEntree;
        e=c/nEntree;
        f=a/SQR(nEntree);
        if (d<e) 
        {
            *i=b+e+(f*nEntree*(nEntree+1)+(2*nEntree-d+1)*d)/2;
            return 0;
        };
        *i=b+d+(f*nEntree*(nEntree+1)+(2*nEntree-e+1)*e)/2;    
        return 1;
    };

    *i=b+nRules*nEntree*(nEntree+1)/2+a-c;
    return 1;
};

gaussian* gaus_Init(gaussian *_thisref, const mxArray *pmiso,vector nu, vector nd, 
                    vector ny)
{
    gaussian *thisref;    
    vector tamp;
    vectorI tampI;
    int a,i,j;
    if (_thisref==NULL)
    {
        thisref=(gaussian*)MyMalloc(sizeof(gaussian));        
    }
    else thisref=_thisref;

    syst_Init((syst*)thisref,pmiso,nu,nd,ny);

    thisref->type='g';
    
    thisref->nRules=(int)GetDouble(pmiso,"n_rules");

    thisref->Centers  =getVector(pmiso,"centers"   );
    thisref->Variances=getVector(pmiso,"ivariances");
    thisref->Linears  =getVector(pmiso,"linears"   );
    thisref->optimParamsOrig=getVector(mxGetField(pmiso,0,"mapping"),"optimparams");

// renumber the "optimparams" matrix to match "our" definition
    j=0;
    for (i=0; i<thisref->optimParams.n; i++)
    {
        a=thisref->optimParams.p[i];
        if (params_convert(&a,thisref->nRules,thisref->nEntree))
        {
            thisref->optimParams.p[j]=a;
            j++;
        };
    };
    if (j!=0)
    {
       vi_Init(&tampI,j);
       memcpy(tampI.p,thisref->optimParams.p,tampI.n*sizeof(int));
    } else tampI.n=0;
    vi_Terminate(&thisref->optimParams);
    thisref->optimParams=tampI;

// for the 'eval' and 'jacob???' functions :
    ca_Init(&thisref->CA,thisref->nEntree,1);
    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)
{
    mxArray *array_ptr;
    double *tamp;
    ca_Terminate(&thisref->CA);
    v_Terminate(&thisref->xMinC);
    v_Terminate(&thisref->memb);
    v_Terminate(&thisref->y);
};

double gaus_eval(gaussian *thisref,vector *in)
                                        /*gaussian membership*/
{
    double out,temp;
    int i,j,r,s,nRules,nEntree;
    dataPtr xMinC,memb,y;

    if (ca_test(&thisref->CA,in->p,NULL)) return *thisref->CA.out.p;
    nRules=thisref->nRules;
    nEntree=thisref->nEntree;
    xMinC=thisref->xMinC.p;
    memb=thisref->memb.p;
    y=thisref->y.p;
    
    /* 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;
    ca_update(&thisref->CA,in->p,&out);
    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;
    /* 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)
{
    hyperbolic *thisref;
    if (_thisref==NULL) 
    {
        thisref=(hyperbolic*)MyMalloc(sizeof(hyperbolic));        
    }
    else thisref=_thisref;

    gaus_Init((gaussian*)thisref,pmiso,nu,nd,ny);
    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));
};

/* =======================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*)MyMalloc(sizeof(StateMatrix));        
    }
    else thisref=_thisref;

    pa=mxGetField(mx,0,"set_points");
    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 set_points matrix must be of the form [1:n]");
        thisref->setPoints=j;
    }
    *setPoints=thisref->setPoints;

    thisref->sampTime=GetDouble(FcnParam,"sampTime");
    thisref->horizon=_horizon;

    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+1)*sizeof(double); 
            //le x est celui de matlab
    v_Init(&thisref->v,(cntrl->nIn+cntrl->nOut)*(thisref->max_time+_horizon+3)); 
            //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)
{
    int k,i,j;

    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*)MyMalloc(sizeof(DerivMatrix));        
    }
    else thisref=_thisref;

    thisref->shift=(cntrl->nOut+plant->nOut)*(_horizon+2);
    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)
};


