/*
 * simulation_error.c: multiplestep-ahead adaptative controller
 *
 *
 * Copyright (c) 1990-97, by Vanden Berghen Frank.
 * All Rights Reserved
 * $Revision 1.1 $
 *
 */

#undef my_ctrl_stand_alone 

#include "ICsimstruc.h"
#include <memory.h>
#include <string.h> // for memmove (microsoft bug)
#include <math.h>
#include "mex.h"
#include "ICctrl_classes.h"

//  these 5 variables are fake: there are only here to be able to use ICctrl_classes.c
int save;
debugClass *d_bug;
mimo *plant,*cntrl;
SimStruct *S;


static vector *entree_work;
static mimo *systeme;

// =======================SimpleStateMatrix object==================================

typedef struct SimpleStateMatrixTag
        {
            vector v;
            int xsize,max_time;
        }SimpleStateMatrix;
SimpleStateMatrix *X;

SimpleStateMatrix * sst_Init(SimpleStateMatrix *_thisref,const mxArray *x0)
{
	char buf[100];
    int i,j,a=0;
    SimpleStateMatrix *thisref;
    if (_thisref==NULL) 
    {
        thisref=(SimpleStateMatrix*)mxMalloc(sizeof(SimpleStateMatrix));
        mexMakeMemoryPersistent(thisref);
    }
    else thisref=_thisref;

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

	if ((mxGetM(x0)!=systeme->nIn+systeme->nOut)||
		(mxGetN(x0)!=a))
	{
		sprintf((char*)&buf,"x0: must be a (%i)x(%i) matrix",
					systeme->nIn+systeme->nOut,a);
		mexErrMsgTxt((char*)&buf);
	};

	thisref->xsize=(systeme->nIn+systeme->nOut)*(thisref->max_time)*sizeof(double);
	
    v_Init(&thisref->v,(systeme->nIn+systeme->nOut)*(thisref->max_time+1));
									//initialisation de mon x	
	memcpy(thisref->v.p,mxGetPr(x0),thisref->xsize);
    return thisref;
};

void sst_Terminate(SimpleStateMatrix *thisref)
{
    v_Terminate(&thisref->v);
};

double sst_Get(SimpleStateMatrix *thisref,int k, int i, int kind)
{
    int base=(k+thisref->max_time)*(systeme->nIn+systeme->nOut);

	if (base<0) error("SimpleStateMatrix access error");
    if (kind==OUT) return thisref->v.p[base+i+systeme->nIn];
    else return thisref->v.p[base+i];
};

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

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

           thisref->v.p[(k+thisref->max_time)*(systeme->nIn+systeme->nOut)+i]=value;
           break;
   case YS://**/ if (i>=systeme->nOut) error("sy");

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

           thisref->v.p[(k+thisref->max_time)*(systeme->nIn+systeme->nOut)+i+
                        systeme->nIn]=value;
           break;
   }
};

void sst_InitTimeStep(SimpleStateMatrix *thisref,const double *U)
{
	int i;
	for (i=0; i<systeme->nIn; i++) sst_Set(thisref,-1,i,US,U[i]);
};

void sst_EndTimeStep(SimpleStateMatrix *thisref, double *Y)
{
	int a=(thisref->max_time)*(systeme->nIn+systeme->nOut)+systeme->nIn;
	memcpy(Y,thisref->v.p+a,systeme->nOut*sizeof(double));
    memmove(thisref->v.p,(thisref->v.p+systeme->nIn+systeme->nOut),thisref->xsize);
};

//=====================================MEX function=======================================

void userTerminate()
{
	 sst_Terminate(X);			   mxFree(X);
    mimo_Terminate(systeme);         mxFree(systeme);
       v_Terminate(entree_work);   mxFree(entree_work);
};

void userInit(const mxArray *params, const mxArray *x0)
{
    int i,max_entree=0;
    systeme=mimo_Init2(NULL,params,PLANT);

	X=sst_Init(NULL,x0);

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

void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
	/* 
	  inputs: *"external" object(systeme) used to make the predictions
			  *u(input) of the system
			  *x0, the initial state-matrix
	  output: Y for each time step
	*/

	long int horizon,t;
	int i;
	double *u,*y;

	if ((nlhs!=1)||(nrhs!=3)) mexErrMsgTxt("wrong number of arguments.");
	userInit(prhs[0],prhs[2]);
	if (mxGetM(prhs[1])!=systeme->nIn) 
		mexErrMsgTxt("the Us(second argument) have not a good size");
	horizon=mxGetN(prhs[1]);
	u=mxGetPr(prhs[1]);	
	plhs[0]= mxCreateDoubleMatrix(systeme->nOut,horizon,mxREAL);
	y=mxGetPr(plhs[0]);

	for (t=0; t<horizon; t++)
	{
		sst_InitTimeStep(X,u);
		for (i=0; i<systeme->nOut; i++)
        {
		    sst_CreateEntree(X,systeme,i,-1,entree_work);
            sst_Set(X,0,i,YS,mimo_eval(systeme,i,entree_work));
		};
		sst_EndTimeStep(X,y);
		u+=systeme->nIn;
		y+=systeme->nOut;
	}
	userTerminate();
};
