/*
 * controller.c: multiplestep-ahead adaptative controller
 *
 *
 * Copyright (c) 1990-97, by Vanden Berghen Frank.
 * All Rights Reserved
 * $Revision 1.1 $
 *
 * there is 2 ways to compile this file. To generate :
 * - a S-function: compile normally: type : 
		mex -O ICcontroller.c ICctrl_classes.c
 * - a simple MEX-file: compile with the compiler-variable "stand_alone" defined: type:
		mex -O -Dstand_alone ICconroller.c ICctrl_classes.c ICsimstruc.c
 */

//#define stand_alone 1

#ifdef my_ctrl_stand_alone 
#include "ICsimstruc.h"
#else
#define S_FUNCTION_NAME controller
#include "simstruc.h"
#endif

/*
 * Need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */

#include <memory.h>
#include <math.h>
#include "ICctrl_classes.h"

 /*=========================*
 * heart of the controller *
 *=========================*/

typedef struct saveTag
        { int a,n,y;
          double z,w;
          vector *d,*e,*f,*o,*p,*q,*r,*s,*g,*h,*b;
          vectorI *t;
          mimo *i,*j;
          StateMatrix *k;
          DerivMatrix *l;
          debugClass *v;
		  filterT *m;
		  time_T c;
        } _save;

int horizon,save,toDo;
time_T Tstart;
double ethaGradiant;
vector *entree_work,*Q,*R,*alphas,*alphasB,
       *betas,*betasB,*currentParams,*jacobien;
vectorI *ipiv;
mimo *cntrl;
mimo *plant;
StateMatrix *X;
DerivMatrix *deriv;
debugClass *d_bug;
filterT *filter;

SimStruct *S;

void LoadData()
{
// rÈinitialise les donnÈes ý chaque appel du conntrÙleur.
    _save *s=(_save*)ssGetPWorkValue(S,0);
    horizon=s->a; jacobien=s->b; Tstart=s->c;
    entree_work=s->d; Q=s->e; R=s->f; 
    cntrl=s->i; plant=s->j; X=s->k; deriv=s->l; save=s->n; filter=s->m;
    alphas=s->o; alphasB=s->p; betas=s->q; betasB=s->r;
    currentParams=s->s; ipiv=s->t; d_bug=s->v; 
	toDo=s->y; ethaGradiant=s->z; 
};

void FirstSaveData()
{
// sauve les donnÈes pour les prochains appels du contrÙleur.
    _save *s=(_save*)mxMalloc(sizeof(_save));
    mexMakeMemoryPersistent(s);
    ssSetPWorkValue(S,0,(void*)s);
    s->a=horizon; s->b=jacobien; s->c=Tstart;
    s->d=entree_work; s->e=Q; s->f=R;
    s->i=cntrl; s->j=plant; s->k=X; s->l=deriv; s->n=save; s->m=filter;
    s->o=alphas; s->p=alphasB; s->q=betas; s->r=betasB;
    s->s=currentParams; s->t=ipiv; s->v=d_bug;
	s->y=toDo; s->z=ethaGradiant; 
};

void Terminate_lazy_learning()
{
	const mxArray *pa=mxGetField(mxGetField(ssGetSFcnParam(S,0),0,"plant_model"),0,"system");
	mxArray *tmp,*tmp1;
	int i;
	lazy *ll;

	for (i=0; i<plant->nOut; i++)
	{
		ll=(lazy*)mimo_get(plant,i);
		if (ll->type=='a')
		{
			tmp=mxGetCell(mxGetField(pa,0,"mapping"),i);
			tmp1=mxGetField(tmp,0,"examples_y");
			v_exactshape(&ll->Y);
			mxSetPr(tmp1,ll->Y.p);
			mxSetM(tmp1,ll->Y.n);
			tmp1=mxGetField(tmp,0,"examples_x");
			t_exactshape(&ll->X);
			mxSetPr(tmp1,*ll->X.p);
			mxSetM(tmp1,ll->X.m);
		};
	};
};

void userTerminate()
{
// Cette fonction est appellÈe quand le contrÙleur est arrÍtÈ.
// elle libËre la mÈmoire.
    LoadData();
	if ((toDo&4)!=0)
	{
     fil_Terminate(filter);
	};

	if ((toDo&2)!=0) 
	{
		 Terminate_lazy_learning();
	};
      db_Terminate(d_bug);         mxFree(d_bug);
    mimo_Terminate(cntrl);         mxFree(cntrl);
    mimo_Terminate(plant);         mxFree(plant);
      st_Terminate(X);             mxFree(X);
     der_Terminate(deriv);         mxFree(deriv);
       v_Terminate(entree_work);   mxFree(entree_work);
	   v_Terminate(jacobien);	   mxFree(jacobien);
    if ((toDo&1)!=0)
    {    
       v_Terminate(currentParams); mxFree(currentParams);
       v_Terminate(alphas);        mxFree(alphas);
       v_Terminate(alphasB);       mxFree(alphasB);
       v_Terminate(betas);         mxFree(betas);
       v_Terminate(betasB);        mxFree(betasB);
      vi_Terminate(ipiv);          mxFree(ipiv);
    };
                                   mxFree(Q);
                                   mxFree(R);
                                   mxFree(ssGetPWorkValue(S,0));
};

void userInit()
{
// fonction appelÈe pour l'initialisation des variables internes
// ý partir des donnÈes contenues dans la variable passÈe en paramËtre
// au contrÙleur.
    const mxArray *params=ssGetSFcnParam(S,0);
    int i,nPar,max_entree=0,sum=0;
    int setPoints;

    Q=(vector*)mxMalloc(sizeof(vector));
    mexMakeMemoryPersistent(Q);
    R=(vector*)mxMalloc(sizeof(vector));
    mexMakeMemoryPersistent(R);

    horizon=(int)(GetDoublePlus(params,"horiz",5))+1;
    toDo=(int)(GetDoublePlus(params,"toDo",0));
    if ((toDo&1)==0) ethaGradiant=GetDouble(params,"etha");
	Tstart=(time_T)(GetDoublePlus(params,"lock_params",0));
    *Q=getVector(params,"Q");
    *R=getVector(params,"R");
        
    save=(int)(GetDoublePlus(params,"save",0));

    cntrl=mimo_Init(NULL,params,"controller",CONTROLLER);
    plant=mimo_Init(NULL,params,"plant_model",PLANT);
	if ((toDo&2)==0) plant->numTParams=0;

    X    =  st_Init(NULL,params,&setPoints,horizon);
    deriv= der_Init(NULL,setPoints,horizon,cntrl->numTParams);

    // vecteur de travail utilisÈ partout comme entrÈe des MISOs
    for (i=0; i<cntrl->nOut; i++)
        max_entree=MAX(max_entree,mimo_get(cntrl,i)->nEntree);
    for (i=0; i<plant->nOut; i++)
        max_entree=MAX(max_entree,mimo_get(plant,i)->nEntree);
    entree_work=v_Init(NULL,max_entree);

    nPar=cntrl->numTParams;
    if ((toDo&1)!=0)
    {    
    // pour lev-Mar
        currentParams=v_Init(NULL,nPar);
        alphas =v_Init(NULL,nPar*nPar);
        alphasB=v_Init(NULL,nPar*nPar);
        betas  =v_Init(NULL,nPar);
        betasB =v_Init(NULL,nPar);
        ipiv   =vi_Init(NULL,nPar);
    };

	for (i=0; i<cntrl->nOut; i++) sum+=mimo_get(cntrl,i)->nEntree;
	for (i=0; i<plant->nOut; i++) sum+=mimo_get(plant,i)->nEntree;
	jacobien=v_Init(NULL,horizon*sum);

	if ((toDo&4)!=0)
	{
		filter=fil_Init(NULL,params,X);
	};

    d_bug=db_Init(NULL,toDo,params,X,deriv);
};

double evalCou()
{
// calcule la valeur de la fonction de cošt J.
    double sum2,sum=0;
    int t,i,j;

    for (t=0; t<horizon; t++)
    {
        for (i=0; i<plant->nOut; i++)
        {
            sum2=0;
            for (j=0; j<plant->nOut; j++) 
                sum2+=(Q->p[i*plant->nOut+j])*
                      (st_Get(X,t,j,OUT,PLANT)-st_Get(X,t,j,SP,PLANT));
            sum+=(st_Get(X,t,i,OUT,PLANT)-st_Get(X,t,i,SP,PLANT))*sum2;
        };
        for (i=0; i<cntrl->nOut; i++)
        {
            sum2=0;
            for (j=0; j<cntrl->nOut; j++)
                sum2+=(R->p[i*cntrl->nOut+j])*
                      (st_Get(X,t,j,IN,PLANT)-st_Get(X,t-1,j,IN,PLANT));
            sum+=(st_Get(X,t,i,IN,PLANT)-st_Get(X,t-1,i,IN,PLANT))*sum2;
        };
    };
    return sum;
};

int gaussj(dataPtr a,dataPtr b,int n)
{
    // gauss-jordan elimination for A*X=B.
    // the resulting X is in B.
    // A is destroyed.

    int i,j,k,irow,icol;
    double dum,pivinv,big,tamp;

    for (j=0; j<n; j++) ipiv->p[j]=0;
    for (i=0; i<n; i++)
    {
        big=0;
        for (j=0; j<n; j++)
            if (ipiv->p[j]!=1)
                for (k=0; k<n; k++)
                    if (ipiv->p[k]==0)
                    {
                        tamp=fabs(a[j*n+k]);
                        if (tamp>=big) 
                        {
                            big=tamp; irow=j; icol=k;
                        }
                    }
                    else if (ipiv->p[k]>1) 
                            {
                                //error("singular matrix-1");
                                return 1;
                            }
        ++(ipiv->p[icol]);
        if (irow!=icol)
        {
            for (j=0; j<n; j++) SWAP(a[irow*n+j],a[icol*n+j]);
            SWAP(b[irow],b[icol]);
        }
        tamp=a[n*icol+icol];
        if (tamp<1e-5) 
        {
            // error("singular matrix-2");
            return 1;
        }
        pivinv=1/tamp;
        a[n*icol+icol]=1;
        for (j=0; j<n; j++) a[n*icol+j]*=pivinv;
        b[icol]*=pivinv;
        for (j=0; j<n; j++)
            if (j!=icol)
            {
                dum=a[j*n+icol];
                a[j*n+icol]=0;
                for (k=0; k<n; k++)
                    a[j*n+k]-=a[icol*n+k]*dum;
                b[j]-=b[icol]*dum;
            };
    };
    return 0;
};

void closed_loop()
{
// simule la boucle fermÈe sur un horizon h
    int i,t;
	double tmp;

    for (t=0; t<horizon; t++)
    {
        for (i=0; i<cntrl->nOut; i++)
        {
            st_CreateEntree(X,cntrl,i,t,entree_work);
            st_Set(X,t,i,US,mimo_eval(cntrl,i,entree_work));
        };		
        if (t<horizon-1) 
            for (i=0; i<plant->nOut; i++)
            {
                st_CreateEntree(X,plant,i,t,entree_work);
				tmp=mimo_eval(plant,i,entree_work);
				if (((toDo&4)!=0)&&(!filter->initial)) tmp+=filter->saveF.p[i];
                st_Set(X,t+1,i,YS,tmp);
            };
    };
};

void jacob_calculate()
{
    int i,k,t,index=0;

    for (t=0; t<horizon; t++)
    {
        for (i=0; i<cntrl->nOut; i++)
        {
            st_CreateEntree(X,cntrl,i,t,entree_work);
			for (k=0; k<mimo_get(cntrl,i)->nEntree; k++) 
				jacobien->p[index++]=mimo_jacobinput(cntrl,i,k,entree_work);
        };		
        if (t<horizon-1) 
            for (i=0; i<plant->nOut; i++)
            {
                st_CreateEntree(X,plant,i,t,entree_work);
				for (k=0; k<mimo_get(plant,i)->nEntree; k++) 
					jacobien->p[index++]=mimo_jacobinput(plant,i,k,entree_work);
            };
    };
};

void execute(int what,const double *x,double *yc,const double *uc)
{
    int t,i,j,k,p,nPar,iter_good=2,iter_bad=20,index;
//	int entree;
	time_T time=ssGetT(S);
    double sum,sum2,tamp,cou,newCou,etha=10;
	const double lltreshold=0.001;
	table *Xtbl;

    nPar=cntrl->numTParams;

    i=(cntrl->nIn+cntrl->nOut)*(X->max_time-1);
    /* init stuff */

// si les sorties U ont dÈjý ÈtÈ calculÈes alors les renvoyer et sortir.
    if ((X->CTime!=-3)&&
		(X->CTime==time)&&
        ((toDo&2)==0)&&
        (memcmp(x+i+cntrl->nOut,uc,cntrl->nIn*sizeof(double))==0)&& // meme entree
        (memcmp(X->v.p+(cntrl->nIn+cntrl->nOut),x,X->xsize)==0)		// meme X
       )
    {
        db_saveALLagain(d_bug);
        memcpy(yc,x+i,cntrl->nOut*sizeof(double));
        return;
    };

	if (((toDo&2)!=0)&&(X->CTime!=-3))
	{
		// adapation de la base de donnÈes pour le lazy
		for (i=0; i<plant->nOut; i++)		
			if (mimo_get(plant,i)->type=='a')
			{
				tamp=uc[i+X->setPoints];
				if (fabs(tamp-st_Get(X,1,i,OUT,PLANT))/tamp>lltreshold)
				{
					st_CreateEntree(X,plant,i,1,entree_work);
					Xtbl=&((lazy*)mimo_get(plant,i))->X;
					t_extend(Xtbl);	
					for (i=0; i<Xtbl->n; i++) Xtbl->p[i][Xtbl->m-1]=entree_work->p[i];
					t_extend(&((lazy*)mimo_get(plant,i))->C);
					v_extend(&((lazy*)mimo_get(plant,i))->Y); 
					((lazy*)mimo_get(plant,i))->Y.p[Xtbl->m-1]=tamp;
				};
			};
	};

	// initialisation du filtre
	if (((toDo&4)!=0)&&(X->CTime!=-3)) fil_initFromMatlab(filter,uc);

    // init X
    st_InitFromMatlab(X,x,uc,time);
    
if ((time>Tstart)&&(time>X->sampTime*X->max_time)){ // to lock the params

    // do closed loop
    closed_loop();

	jacob_calculate();

	// calcul des dÈrivÈes du rÈgulateur par rapport ý chacun des paramËtres
    // pour t variant de k ý k+horizon (k=0 ici).
    for (p=0; p<cntrl->numTParams; p++)
    {
		index=0;
		for (i=0; i<plant->nOut; i++) der_Set(deriv,p,0,i,YS,0);
		for (t=0; t<horizon; t++)
        {
            // (dui/dp) 
            for (i=0; i<cntrl->nOut; i++)
            {
                st_CreateEntree(X,cntrl,i,t,entree_work);
                sum=mimo_jacobparam(cntrl,i,p,entree_work);
//              entree=0;
                for (j=0; j<mimo_get(cntrl,i)->ny.n; j++)
                    for (k=0; k<mimo_get(cntrl,i)->ny.p[j]; k++)
                    {
                        tamp=der_Get(deriv,p,t-k-1,j,OUT,CONTROLLER);
                        if (tamp!=0)
                            sum+=tamp*jacobien->p[index];
//                               mimo_jacobinput(cntrl,i,entree,entree_work);
//                      entree++; 
						index++;
                    };
                for (j=0; j<mimo_get(cntrl,i)->nu.n; j++)
                    for (k=0; k<mimo_get(cntrl,i)->nu.p[j]; k++)
                    {
						tamp=der_Get(deriv,p,t-k-(mimo_get(cntrl,i)->nd.p[j]),j,IN,CONTROLLER);
                        if (tamp!=0)
                            sum+=tamp*jacobien->p[index];
//                               mimo_jacobinput(cntrl,i,entree,entree_work);
//                      entree++; 
						index++;
                    };
                der_Set(deriv,p,t,i,US,sum);
            };

            // (dyi/dp)
            if (t<horizon-1) for (i=0; i<plant->nOut; i++)
            {
                sum=0; 
//              entree=0; st_CreateEntree(X,plant,i,t,entree_work);
                for (j=0; j<mimo_get(plant,i)->ny.n; j++)
                    for (k=0; k<mimo_get(plant,i)->ny.p[j]; k++)
                    {
                        tamp=der_Get(deriv,p,t-k,j,OUT,PLANT);
                        if (tamp!=0)
                            sum+=tamp*jacobien->p[index];
//                               mimo_jacobinput(plant,i,entree,entree_work);
//                      entree++; 
						index++;
                    };
                for (j=0; j<mimo_get(plant,i)->nu.n; j++)
                    for (k=0; k<mimo_get(plant,i)->nu.p[j]; k++)
                    {
                        tamp=der_Get(deriv,p,t-k-(mimo_get(plant,i)->nd.p[j])+1,j,IN,PLANT);
                        if (tamp!=0)
                            sum+=tamp*jacobien->p[index];
//                               mimo_jacobinput(plant,i,entree,entree_work);
//                      entree++; 
						index++;
                    };
                der_Set(deriv,p,t+1,i,YS,sum);
            };
        };        
    };

    if ((toDo&1)==0)
    {
// gradiant descent algorithm
        for (p=0; p<nPar; p++)
        {
            sum=0;
            for (t=0; t<horizon; t++)
            {
                for (i=0; i<X->setPoints; i++)
                {
                    sum2=0;
                    for (j=0; j<X->setPoints; j++) 
                        sum2+=Q->p[i*X->setPoints+j]*der_Get2(deriv,p,t,j,YS);
                    sum+=(st_Get(X,t,i,OUT,PLANT)-st_Get(X,t,i,SP,PLANT))*sum2;
                };
                for (i=0; i<cntrl->nOut; i++)
                {
                    sum2=0;
                    for (j=0; j<cntrl->nOut; j++)
                        sum2+=R->p[i*cntrl->nOut+j]*
                              (der_Get2(deriv,p,t,j,US)-der_Get2(deriv,p,t-1,j,US));
                    sum+=(st_Get(X,t,i,IN,PLANT)-st_Get(X,t-1,i,IN,PLANT))*sum2;
                };
            };
            mimo_setParam(cntrl,p,mimo_getParam(cntrl,p)-ethaGradiant*sum);
        };
    }
    else
    {
// levenberg-marquardt algorithm

        // calculer alphasB et betasB
        for (p=0; p<nPar; p++)
        {
            betasB->p[p]=0;
            for (j=0; j<nPar; j++) alphasB->p[p*nPar+j]=0;
            for (t=0; t<horizon; t++)
            {
                if (t!=0) for (i=0; i<plant->nOut; i++)
                {
                    sum2=0;
                    for (j=0; j<plant->nOut; j++) 
                        sum2+=Q->p[i*plant->nOut+j]*der_Get2(deriv,p,t,j,YS);
                    betasB->p[p]+=(st_Get(X,t,i,OUT,PLANT)-st_Get(X,t,i,SP,PLANT))*sum2;
                    for (j=0; j<nPar; j++)
                        alphasB->p[p*nPar+j]+=der_Get2(deriv,j,t,i,YS)*sum2;
                };
                for (i=0; i<cntrl->nOut; i++)
                {
                    sum2=0;
                    for (j=0; j<cntrl->nOut; j++)
                        sum2+=R->p[i*cntrl->nOut+j]*
                              (der_Get2(deriv,p,t,j,US)-der_Get2(deriv,p,t-1,j,US));
                    betasB->p[p]+=(st_Get(X,t,i,IN,PLANT)-st_Get(X,t-1,i,IN,PLANT))*sum2;
                    for (j=0; j<nPar; j++)
                        alphasB->p[p*nPar+j]+=(der_Get2(deriv,j,t,i,US)-der_Get2(deriv,j,t-1,i,US))*sum2;
                };
            };
        };

        /*
        //start_debug 
        for (i=0; i<nPar; i++)
            for (j=0; j<nPar; j++)
                alphasB->p[i*nPar+j]=((i==j)?100:0);
        //end_debug
        */

        cou=evalCou();

        //sauver les paramËtres courants
        for (p=0; p<nPar; p++)
            currentParams->p[p]=mimo_getParam(cntrl,p);
        do
        {
            do
            {
                memcpy(alphas->p,alphasB->p,alphasB->n*sizeof(double));
                memcpy(betas->p,betasB->p,betasB->n*sizeof(double));

                for (p=0; p<nPar; p++)
                    alphas->p[p*nPar+p]+=etha;

                i=gaussj(alphas->p,betas->p,nPar);
                if (i==1) etha*=10; // il y a matrice singuliËre.

            } while ((i==1)&&(etha<1e5));
            
            if (i==0)
            {
                //mettre a jour les parametres
                for (p=0; p<nPar; p++)
                    mimo_setParam(cntrl,p,currentParams->p[p]-betas->p[p]);

                closed_loop(); // nÈcessaire pour calculer newCou
                newCou=evalCou();

                if (newCou>=cou)
                {
                    etha*=10;
                    iter_bad--;
                    // pas de mise a jour de currentParams
                }
                else
                {
                    etha*=.1;
                    iter_good--;
                    if (iter_good>0)
                    {
                        cou=newCou;
                        //sauver le nouveau currentParams
                        for (p=0; p<nPar; p++)
                            currentParams->p[p]-=betas->p[p];
                    };
                }
            }
        } while ((iter_good>0)&&(iter_bad>0)&&(etha<1e5)&&(i==0));
        db_saveEtha(d_bug,etha);
    };	

};//end lock_params if (time<Tstart)

	db_saveALL(d_bug);

    // calculer les sorties US en fonction des nouveaux parametres 
    for (i=0; i<cntrl->nOut; i++)
    {
        st_CreateEntree(X,cntrl,i,0,entree_work);
        st_Set(X,0,i,US,mimo_eval(cntrl,i,entree_work));
    };

    if (yc!=NULL)
        memcpy(yc,
               X->v.p+(cntrl->nIn+cntrl->nOut)*X->max_time,
               cntrl->nOut*sizeof(double));

};

void findvalue1(int *nOut,int *nIn,int *maxtime, int *nSample)
{
// cette fonction est appelÈe uniquement lorsque simulink demande la
// taille des vecteurs d'entrÈe et de sortie ý l'initialisation du modËle.
    vector nu,nd,ny;
    int i;
    const mxArray *tmp=mxGetField(mxGetField(ssGetSFcnParam(S,0)
                                             ,0,"controller")
                                  ,0,"system");
    *nOut=(int)GetDouble(tmp,"n_out");
    *nIn=(int)GetDouble(tmp,"n_in");

    // if (((int)GetDouble(ssGetSFcnParam(S,0),"toDo")&2)!=0) *nSample=2; else 
	*nSample=1;

    *maxtime=0;
    tmp=mxGetField(mxGetField(ssGetSFcnParam(S,0),
                              0,"controller"),                              
                   0,"dynamics");
    nu=getVector(tmp,"nu");
    nd=getVector(tmp,"nd");
	ny=getVector(tmp,"ny");
    for (i=0; i<nu.n; i++)
        *maxtime=MAX((int)(nu.p[i]+nd.p[i]),*maxtime);
    for (i=0; i<ny.n; i++)
        *maxtime=MAX((int)(ny.p[i])+1,*maxtime);
    tmp=mxGetField(mxGetField(ssGetSFcnParam(S,0),
                              0,"plant_model"),
                   0,"dynamics");
    nu=getVector(tmp,"nu");
    nd=getVector(tmp,"nd");
	ny=getVector(tmp,"ny");
    for (i=0; i<nu.n; i++)
        *maxtime=MAX((int)(nu.p[i]+nd.p[i])-1,*maxtime);
    for (i=0; i<ny.n; i++)
        *maxtime=MAX((int)(ny.p[i]),*maxtime);
	//(*maxtime)--;
};

void findvalue2(int *nSample,double *sampleT1)
{
// cette fonction est appelÈe uniquement lorsque simulink demande 
// la dÈfinition des pÈriodes d'appel du contrÙleur ý 
// l'initialisation du modËle.
    const mxArray *param=ssGetSFcnParam(S,0);

    *sampleT1=GetDouble(param,"sampTime");
    *nSample=1;
};


/*====================*
 * S-function methods *
 *====================*/

/* Function: mdlInitializeConditions ==========================================
 * Abstract:
 * In this function, you should initialize the continuous and discrete
 * states for your S-function block.  The initial states are placed
 * in the x0 variable.  You can also perform any other initialization
 * activities that your S-function may require.
 */

static void mdlInitializeConditions(real_T *x0, SimStruct *s)
{
	char buf[100];
	vector v;
	int a,b,i;
	S=s;
	userInit();
	v=getVector(ssGetSFcnParam(S,0),"x0");
	if (v.n<X->xsize/(int)sizeof(double)) 
	{
		if (v.n==(cntrl->nIn+cntrl->nOut))
		{
			a=(cntrl->nIn+cntrl->nOut); b=0;
			for (i=0; i<X->max_time; i++)
			{
				memcpy(x0+b,v.p,a*sizeof(double)); b+=a;
			};
		} else
		{
			sprintf((char*)&buf,"params.x0: must be a ((1 or %i)x(%i)) matrix",
					cntrl->nIn+cntrl->nOut,X->max_time);
			error((char*)&buf);
		}
	}
	else memcpy(x0,v.p,X->xsize);

	FirstSaveData();
};

#ifndef stand_alone 

	/* Function: mdlInitializeSizes ===============================================
	 * Abstract:
	 * The sizes information is used by SIMULINK to determine the S-function 
	 * block's characteristics (number of inputs, outputs, states, etc.).
	 */
	static void mdlInitializeSizes(SimStruct *s)
	{
		int CntrlnOut, CntrlnIn, max_time, nSample;
    
		S=s;

		ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
		if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
			/* Return if number of expected != number of actual parameters */
			return;
		}

		findvalue1(&CntrlnOut,&CntrlnIn,&max_time,&nSample);

		ssSetNumContStates(    S, 0);   /* number of continuous states           */

		ssSetNumDiscStates(    S, max_time*(CntrlnIn+CntrlnOut));
									// number of discrete states
		ssSetNumInputs(        S, CntrlnIn);
									// number of inputs
		ssSetNumOutputs(       S, CntrlnOut);
									// number of outputs
		ssSetDirectFeedThrough(S, 0);   /* direct feedthrough flag               */

		ssSetNumSampleTimes(   S, nSample);     
									// number of sample times
		ssSetNumRWork(         S, 0);   /* number of real work vector elements   */
		ssSetNumIWork(         S, 0);   /* number of integer work vector elements*/
		ssSetNumPWork(         S, 1);   /* number of pointer work vector elements*/
		ssSetNumModes(         S, 0);   /* number of mode work vector elements   */
		ssSetNumNonsampledZCs( S, 0);   /* number of nonsampled zero crossings   */
		ssSetOptions(          S, 0);   /* general options (SS_OPTION_xx)        */
	}

	/* Function: mdlInitializeSampleTimes =========================================
	 * Abstract:
	 *
	 * This function is used to specify the sample time(s) for your S-function.
	 * You must register the same number of sample times as specified in 
	 * ssSetNumSampleTimes. 
	 */
	static void mdlInitializeSampleTimes(SimStruct *S)
	{
		int nSample;
		double sampleT1,sampleT2;

		findvalue2(&nSample,&sampleT1);

		ssSetSampleTime(S, 0, sampleT1);
		ssSetOffsetTime(S, 0, 0.0);
	};


	/* Function: mdlOutputs =======================================================
	 * In this function, you compute the outputs of your S-function
	 * block. The outputs are placed in the y variable.
	 */
	static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
						   SimStruct *s, int_T tid)
	{
		S=s;
		LoadData();
		if (ssIsSampleHit(s,0,tid)) execute(1,x,y,u);
	};



	/* Function: mdlUpdate ====================================================
	 * This function is called once for every major integration time steP->
	 * Discrete states are typically updated here, but this function is useful
	 * for performing any tasks that should only take place once per 
	 * integration step.
	 */
	static void mdlUpdate(real_T *x, const real_T *u, SimStruct *s, int_T tid)
	{
		S=s;
		LoadData();
		st_UpdateMatlab(X,(double*)x);
	};


	/* Function: mdlDerivatives ===============================================
	 */
	static void mdlDerivatives(real_T *dx, const real_T *x, const real_T *u, 
							   SimStruct *s, int_T tid)
	{
	};

	/* Function: mdlTerminate =====================================================
	 * In this function, you should perform any actions that are necessary
	 * at the termination of a simulation.  For example, if memory was 
	 * allocated in mdlInitializeConditions, this is the place to mxFree it.
	 */
	static void mdlTerminate(SimStruct *s)
	{
		S=s;
		userTerminate();
	}

	/*=============================*
	 * Required S-function trailer *
	 *=============================*/

	#ifdef    MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
	#include "simulink.c"      /* MEX-file interface mechanism */
	#else
	#include "cg_sfun.h"       /* Code generation registration function */
	#endif

#else

	mxArray *ssGetSFcnParam(SimStruct *Si,int f) { return Si->mx; };
	void *ssGetPWorkValue(SimStruct *Si,int f) { return Si->p; };
	double ssGetT(SimStruct *Si) { return Si->timeT; };
	double ssGetTFinal(SimStruct *Si) { return Si->finalT; };
	double ssGetTStart(SimStruct *Si) { return Si->startT; };
	void ssSetPWorkValue(SimStruct *Si,int f,void *st) { Si->p=st; };
	void ssSetErrorStatus(SimStruct *Si,char *msg) { mexErrMsgTxt(msg); };

	void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
	{
		int max_time, nSample;
		mxArray *array_ptr;
		double *pr;

		if (nrhs>=1) S=(SimStruct*)prhs[0];	
		else mexErrMsgTxt("wrong number of arguments.");
		if (nrhs==1)
		{
			if (S!=NULL) userTerminate();
			else mexErrMsgTxt("no memory to free.");
			return;
		}
		if (nlhs!=1) mexErrMsgTxt("you must have 1 ouput.");
		if (nrhs==4)
		{
			if (S!=NULL) mexErrMsgTxt("error during initialisation");
			S=(SimStruct*)mxMalloc(sizeof(SimStruct));
			mexMakeMemoryPersistent(S);
			S->mx=(mxArray *)prhs[1];
			S->startT=mxGetScalar(prhs[2]);
			S->finalT=mxGetScalar(prhs[3]);
			findvalue1(&S->CntrlnOut,&S->CntrlnIn,&max_time,&nSample);
			findvalue2(&nSample,&S->sampTime);
			S->x=(double*)mxMalloc(max_time*(S->CntrlnIn+S->CntrlnOut)*sizeof(double));
			mexMakeMemoryPersistent(S->x);
			mdlInitializeConditions(S->x,S);
			array_ptr= mxCreateDoubleMatrix(1,1,mxREAL);
			pr=(double*)mxGetPr(array_ptr);
			*pr=(double)(long)S;
			plhs[0]=array_ptr;
			return;
		};
		if (nrhs==3)
		{
			if (S==NULL) 
				mexErrMsgTxt("you must initialise first(first arg.==NULL)");
			if (mxGetN(plhs[2])!=S->CntrlnOut)
				mexErrMsgTxt("bad dimension for output argument");
			array_ptr= mxCreateDoubleMatrix(1,S->CntrlnOut,mxREAL);		
			plhs[0]=array_ptr;
			LoadData();
			execute(1,mxGetPr(prhs[1]),mxGetPr(array_ptr),mxGetPr(prhs[2]));
			S->timeT+=S->sampTime;
		};
		mexErrMsgTxt("wrong calling convention.");
	};

#endif