
// $ Revision: 1.2 $ 
//
// ICmain.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 <memory.h>
#include <math.h>
#include "mex.h"
#include "ICclasses.h"

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

int horizon,toDo,memory_type;
vector *entree_work,*jacobien;
table *deltaYs;
mimo *cntrl;
mimo *plant;
StateMatrix *X;
DerivMatrix *deriv;
const mxArray *FcnParam;

typedef struct saveTag
        { int a,b;
          vector *c,*d,*e,*f;
          table *g;
          mimo *h,*i;
          StateMatrix *j;
          DerivMatrix *k;
        } _save;

void LoadData(double a)
{
// réinitialise les données à chaque appel du contrôleur.
    _save *s=(_save*)(unsigned long)a;
    horizon=s->a; toDo=s->b; entree_work=s->c; jacobien=s->f; 
    deltaYs=s->g; cntrl=s->h; plant=s->i; X=s->j; deriv=s->k; 
};

double FirstSaveData()
{
// sauve les données pour les prochains appels du contrôleur.
    _save *s=(_save*)MyMalloc(sizeof(_save));
    s->a=horizon; s->b=toDo; s->c=entree_work; s->f=jacobien;
    s->g=deltaYs; s->h=cntrl; s->i=plant; s->j=X; s->k=deriv;
    return (double)(unsigned long)s;
};

void userTerminate()
{
// Cette fonction est appellée quand le contrôleur est arrêté.
// elle libère la mémoire.
    if (toDo&8)
    {
       t_Terminate(deltaYs);       MyFree(deltaYs);
    };
    mimo_Terminate(cntrl);         MyFree(cntrl);
    mimo_Terminate(plant);         MyFree(plant);
      st_Terminate(X);             MyFree(X);
     der_Terminate(deriv);         MyFree(deriv);
       v_Terminate(entree_work);   MyFree(entree_work);
       v_Terminate(jacobien);      MyFree(jacobien);
};

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.
    
    int i,j,k,max_entree=0,sum=0;
    int setPoints;
    double *deltaYsTmp,*p;
    syst *f;
   
    horizon=(int)(GetDoublePlus(FcnParam,"horizon",5));
    toDo=(int)(GetDoublePlus(FcnParam,"toDo",0));
        
    cntrl=mimo_Init(NULL,FcnParam,"controller",CONTROLLER);
    plant=mimo_Init(NULL,FcnParam,"plant_model",PLANT);

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

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

    if (toDo&8)
    {
        deltaYs=t_Init(NULL,j,plant->nOut);
        p=plant->minmax.p;
        for (i=0; i<plant->nOut; i++)
        {
            f=mimo_get(plant,i);
            deltaYsTmp=deltaYs->p[i];
            for (j=0; j<f->ny.n; j++)
                for (k=0; k<f->ny.p[j]; k++)
                    *(deltaYsTmp++)=(p[((j+plant->nIn)<<1)+1]-p[(j+plant->nIn)<<1])/1.5;
            for (j=0; j<f->nu.n; j++)
                for (k=0; k<f->nu.p[j]; k++)
                *(deltaYsTmp++)=(p[(j<<1)+1]-p[j<<1])/1.5;
        };
    };
};

void closed_loop(const double *steadyerror)
{
// 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));
        };        

        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) tmp+=steadyerror[i];
            st_Set(X,t+1,i,YS,tmp);
        };
    };
};

void execute(const double *x,const double *steadyerror)
{
    int i,j,k,p,index,indexS,t;
    double sum,tamp,maxslopes;
    double *pminmax=plant->minmax.p+(plant->nIn<<1);
    
    // init X
    st_InitFromMatlab(X,x);
    
    // do closed loop
    closed_loop(steadyerror);

    // 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++)
        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);
            for (k=0; k<mimo_get(cntrl,i)->nEntree; k++)
                jacobien->p[k]=mimo_jacobinput(cntrl,i,k,entree_work);

            for (p=0; p<cntrl->numTParams; p++)
            {
                index=0;
                sum=mimo_jacobparam(cntrl,i,p,entree_work);
                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];
                        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]; 
                        index++;
                    };
                der_Set(deriv,p,t,i,US,sum);
            };
        }; // boucle sur cntrl->nOut;

        // (dyi/dp)
        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++) 
            {
                tamp=mimo_jacobinput(plant,i,k,entree_work);
                if (toDo&8)
                {
            //y=mx => dy/dx=m
                    maxslopes=(pminmax[(i<<1)+1]-pminmax[i<<1])/deltaYs->p[i][k];
                    tamp=MIN(maxslopes,MAX(-maxslopes,tamp));
                };
                jacobien->p[k]=tamp;
            };
            for (p=0; p<cntrl->numTParams; p++)
            {
                index=0;
                sum=0;
                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]; 
                        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];
                        index++;
                    };
                der_Set(deriv,p,t+1,i,YS,sum);
            };
        }; // boucle sur plant->nOut
    }; // boucle sur t
};

char params_convert(int *i, int nRules, int nEntree);
int params_backward(int i)
{
    int s=0,sumP=0,sumPold,j=0;
    gaussian *ct;

    do
    { 
       ct=(gaussian*)mimo_get(cntrl,s);
       sumPold=sumP;
       sumP+=ct->optimParamsOrig.n;
       s++;
    } while (i<sumP);

    i=(int)ct->optimParamsOrig.p[i-sumPold];
    params_convert(&i,ct->nRules,ct->nEntree);
    while (ct->optimParams.p[j]!=i) j++;
    return cntrl->indiceParams.p[s-1]+j;
};

void update_cntrl()
{
    mxArray *m=mxGetField(mxGetField(mxGetField(FcnParam,0,"controller"),0,"system"),0,"mapping"),*t1;
    gaussian *ct;
    int i;
    for (i=0; i<cntrl->nMapping; i++)
    {
        ct=(gaussian*)mimo_get(cntrl,i);
        t1=mxGetCell(m,i);
        ct->Centers.p=mxGetPr(mxGetField(t1,0,"centers"));
        ct->Variances.p=mxGetPr(mxGetField(t1,0,"ivariances"));
        ct->Linears.p=mxGetPr(mxGetField(t1,0,"linears"));
    };
};

void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
    char buf[300];
    double *pr,tamp;
    int dim[3],j,time,p;
    mxArray *array_ptr;    
    
    if (nrhs==3)
    {
         memory_type=4;
         FcnParam=prhs[2]; 
         userInit();
    }
    else
    {        
        if (nrhs==1)
        {
            if (nlhs==1)
            {
                memory_type=0;
                FcnParam=prhs[0];
                userInit();
                array_ptr=mxCreateDoubleMatrix(1,1,mxREAL);    
                plhs[0]=array_ptr;
                pr=mxGetPr(array_ptr);
                *pr=FirstSaveData();
                return;
            };
            if (nlhs==0)
            { 
                memory_type=2;
                LoadData(mxGetScalar(prhs[0]));
                userTerminate();
                return;
            };
            mexErrMsgTxt("wrong number of left arguments.");
        };

        if (nrhs!=4) mexErrMsgTxt("wrong number of right arguments.");
        memory_type=1;
        FcnParam=prhs[2]; 
        LoadData(mxGetScalar(prhs[3]));
        update_cntrl();
    };
    if (nlhs!=3) mexErrMsgTxt("wrong number of left arguments.");

    j=mxGetNumberOfElements(prhs[0]);
    if (mxGetNumberOfElements(prhs[0])!=X->xsize/sizeof(double))
    {
        sprintf((char*)&buf,"x: must be a ((%i)x(%i)) matrix",
                cntrl->nIn+cntrl->nOut,X->max_time+1);
        mexErrMsgTxt((char*)&buf);
    };

    if (mxGetNumberOfElements(prhs[1])!=plant->nOut)
        mexErrMsgTxt("bad dimension for anti_steady_state");

    execute(mxGetPr(prhs[0]),mxGetPr(prhs[1]));

    // retourner saved_us
    array_ptr=mxCreateDoubleMatrix(X->horizon+1,cntrl->nOut,mxREAL);
    plhs[1]=array_ptr;
    pr=mxGetPr(array_ptr);

    for (j=0; j<cntrl->nOut; j++)
    {
        for (time=0; time<X->horizon; time++)
            *(pr++)=st_Get(X,time,j,OUT,CONTROLLER);
        *(pr++)=0;
    };

    // retourner saved_ys
    array_ptr=mxCreateDoubleMatrix(X->horizon+1,plant->nOut,mxREAL);
    plhs[2]=array_ptr;
    pr=mxGetPr(array_ptr);
    
    for (j=0; j<plant->nOut; j++)
    {
        for (time=1; time<X->horizon+1; time++)
            *(pr++)=st_Get(X,time,j,OUT,PLANT);
        *(pr++)=0;
    };

    //retourner derivative_table
    dim[0]=X->horizon+1; 
    dim[1]=cntrl->nOut+plant->nOut;
//    dim[2]=cntrl->numTParamsOrig;
    dim[2]=cntrl->numTParams;
    array_ptr=mxCreateNumericArray(3,(int*)&dim,mxDOUBLE_CLASS,mxREAL);
    dim[0]--;
    plhs[0]=array_ptr;
    pr=mxGetPr(array_ptr);

    for (p=0; p<cntrl->numTParams; p++)
    {
        for (j=0; j<plant->nOut; j++)
        {
            for (time=0; time<dim[0]; time++)
                *(pr++)=der_Get(deriv,p,time+1,j,OUT,PLANT);
            *(pr++)=0;
        };
        for (j=0; j<cntrl->nOut; j++)
         {               
            for (time=0; time<dim[0]; time++)
                *(pr++)=der_Get(deriv,p,time,j,OUT,CONTROLLER);
            *(pr++)=0;
        };
    };

/*
    for (p2=0; p2<cntrl->numTParamsOrig; p2++)
    {
        p=params_backward(p2);
        for (j=0; j<plant->nOut; j++)
            for (time=0; time<dim[0]; time++)
                *(pr++)=der_Get(deriv,p,time+1,j,OUT,PLANT);

        for (j=0; j<cntrl->nOut; j++)
            for (time=0; time<dim[0]; time++)
                *(pr++)=der_Get(deriv,p,time,j,OUT,CONTROLLER);
    };
*/

    
//    if (memory_type==4) userTerminate();
};























