#ifndef _WIN32
#include "../include/lminmaxgd.h"
#else
#include "..\include\lminmaxgd.h"

#endif
#include <time.h>
using namespace std;


/////////////////////////////////////////////////////////////////////
/* Example : Consensus between two systems
Two SISO systems are build randomly H1 and H2
This program compute :
1. the optimal controllers P1 and P2 such that H1P1==H2P2=Hmax are given
2. the optimal feedback is given such that (G1F)*G1 <=  [Hmax Hmax; Hmax Hmax ]
with G1=[H1P1 epsilon; epsilon H2P2]

3. the result is given we obtain a system [Hmax Gmax; Gmax Hmax], i.e. the ouptu y1==y2
(consensus is achieved) when v1==v2, tradittionnaly v1==v2=e
*/
#include "..\src\gd.cpp"
#include "..\src\poly.cpp"
#include "..\src\serie.cpp"
#include "..\src\smatrix.cpp"
#include "..\src\tools.cpp"
int Max(int a, int b)
{
     if (a>=b) return a;
     else return b;
}

int Min(int a, int b)
{
     if (a<=b) return a;
     else return b;
}

using namespace std;

int main(void)
{
try{
int x[8][100]; // state
int xopt[8][100]; // optimal state
int u [4][100]; // control
int uopt [4][100]; // optimal control
int z[4][100]; // refrence output
int zeta[8][100]; // co-state
int y[4][100]; // output
int alpha1,alpha2,alpha3,alpha4;
int i,t,k;

int T0=6; // the maximal memory delay
for(i=0;i<8;i++)  // initialisatiopn of all the data
{
    for (t=0;t<100;t++)
    {
        x[i][t]=0;
        xopt[i][t]=0;
        zeta[i][t]=0;
    }
}
// intialisation of u, to test forward simulation
for (t=99;t>=0;t--)  // u1
{
u[0][t]=6;
if (t<=3+T0) u[0][t]=3;
if(t<=1+T0)   u[0][t]=1;
if(t<=0+T0)   u[0][t]=0;
}
for (t=99;t>=0;t--) //u2
{
u[1][t]=4;
if (t<=3+T0) u[1][t]=3;
if(t<=1+T0)   u[1][t]=2;
if(t<=0+T0)   u[1][t]=0;
}

for (t=99;t>=0;t--) //u3
{
u[2][t]=5;
if (t<=3+T0) u[2][t]=3;
if(t<=0+T0)   u[2][t]=0;
}

for (t=99;t>=0;t--) //u4
{
u[3][t]=7;
if (t<=5+T0) u[3][t]=2;
if(t<=0+T0)   u[3][t]=0;
}
// I print all the input u
for(i=0;i<4;i++)
{


printf("u%d = ",i+1);
for(t=T0;t<99;t++)
{
    if(u[i][t]<u[i][t+1]) printf(" + %d d^%d ",u[i][t],t-T0); // only if there is a change I print the monomial, tak care  time 0 is actually the index T0

}
printf("+ %d^+oo\n",u[i][99]);
}

/** Forward Simulation **/
for (t=T0;t<100;t++)
{
    alpha1=2+x[1][t-2]-x[0][t-1]+x[3][t-2]-x[2][t-1]+x[5][t-2]-x[4][t-1]+x[7][t-2]-x[6][t-1];

    x[0][t]=Min(u[0][t],x[0][t-1]+alpha1);

    x[1][t]=x[0][t-3];

    alpha2=alpha1+x[0][t-1]-x[0][t];
    x[2][t]=Min(u[1][t],x[2][t-1]+alpha2);
    x[3][t]=x[2][t-5];
    alpha3=alpha2+x[2][t-1]-x[2][t];
    x[4][t]=Min(u[2][t],x[4][t-1]+alpha3);
    x[5][t]=x[4][t-6];
    alpha4=alpha3+x[4][t-1]-x[4][t];
    x[6][t]=Min(u[3][t],x[6][t-1]+alpha4);
    x[7][t]=x[6][t-4];

}
/** print the state **/
for(i=0;i<8;i++)
{


printf("x%d ",i);
for(t=T0;t<99;t++)
{
    if(x[i][t]<x[i][t+1]) printf(" + %d d^%d ",x[i][t],t-T0);

}


printf("+ %d^+oo\n",x[i][99]);
}


/** Defintion of output reference ***/

for (t=99;t>=0;t--)
{
z[0][t]=6;
if (t<=54+T0) z[0][t]=3;
if(t<=46+T0)   z[0][t]=1;
if(t<=42+T0)   z[0][t]=0;
}
for (t=99;t>=0;t--)
{
z[1][t]=3;
if (t<=54+T0) z[1][t]=2;
if(t<=50+T0)   z[1][t]=1;
if(t<=39+T0)   z[1][t]=0;
}

for (t=99;t>=0;t--)
{
z[2][t]=4;
if (t<=54+T0) z[2][t]=2;
if(t<=50+T0)   z[2][t]=1;
if(t<=48+T0)   z[2][t]=0;
}

for (t=99;t>=0;t--)
{
z[3][t]=2;
if (t<=54+T0) z[3][t]=0;
}
for(i=0;i<4;i++)
{
/** print the reference output **/

printf("z%d = ",i+1);
for(t=T0;t<99;t++)
{
    if(z[i][t]<z[i][t+1]) printf(" + %d d^%d ",z[i][t],t-T0);

}
printf("+ %d^+oo\n",z[i][96]);
}
/** initialisation of the co-state equal to the final value and of the optimal input vector equal to 0 **/

for(i=0;i<4;i++)
{
    for (t=0;t<100;t++)
    {

        zeta[2*i][t]=z[i][99];
        zeta[2*i+1][t]=z[i][99];
        uopt[i][t]=0;
    }
}

for(i=0;i<4;i++)
{

printf("z%d = ",i+1);
for(t=T0;t<99;t++)
{
    if(z[i][t]<z[i][t+1]) printf(" + %d d^%d ",z[i][t],t-T0);

}
printf("+ %d^+oo\n",z[i][99]);
}

for(i=0;i<8;i++)
{


printf("zeta%d ",i);
for(t=T0;t<99;t++)
{
    if(zeta[i][t]<zeta[i][t+1]) printf(" + %d d^%d ",zeta[i][t],t-T0);

}


printf("+ %d^+oo\n",zeta[i][98]);
}


/** Computation  of the optimal control for the first system, the n resources are asumed to be free **/
int n=2;
for (t=96;t>=T0;t--)
{
    zeta[1][t]=Max(z[0][t],zeta[0][t+2]-n);
    zeta[0][t]=zeta[1][t+3];
    uopt[0][t]=zeta[0][t];

}

/** print the optimal input **/
for(i=0;i<4;i++)
{


printf("Uopt%d = ",i+1);
for(t=T0;t<96;t++)
{
    if(uopt[i][t]<uopt[i][t+1]) printf(" + %d d^%d ",uopt[i][t],t-T0);

}
printf("+ %d^+oo\n",uopt[i][96]);
}
/** Defintion of vector beta **/
int beta2tilde[100];
int beta2[100];

/** Simulation with u1opt and u2opt=u3opt=u3opt=0 no firing, it is like if we simulate only the first system **/
for (t=T0;t<100;t++)
{
    alpha1=2+xopt[1][t-2]-xopt[0][t-1]+xopt[3][t-2]-xopt[2][t-1]+xopt[5][t-2]-xopt[4][t-1]+xopt[7][t-2]-xopt[6][t-1];

    xopt[0][t]=Min(uopt[0][t],xopt[0][t-1]+alpha1);


    xopt[1][t]=xopt[0][t-3];

    alpha2=alpha1+xopt[0][t-1]-xopt[0][t];
    beta2tilde[t]=alpha2;                                   /** here just to compare that beta2tilde computed later is the same than alpha 2 **/
    xopt[2][t]=Min(uopt[1][t],xopt[2][t-1]+alpha2);

    xopt[3][t]=xopt[2][t-5];
    alpha3=alpha2+xopt[2][t-1]-xopt[2][t];
    xopt[4][t]=Min(uopt[2][t],xopt[4][t-1]+alpha3);

    xopt[5][t]=xopt[4][t-6];
    alpha4=alpha3+xopt[4][t-1]-xopt[4][t];
    xopt[6][t]=Min(uopt[3][t],xopt[6][t-1]+alpha4);

    xopt[7][t]=xopt[6][t-4];

}

for (t=T0;t<96;t++)
{ // printf("t: %d btilde %d ",t,beta2tilde[t]);  /** here it is alpaha2 which is printed actually **/
   beta2tilde[t]=xopt[1][t-2]-xopt[0][t]+2;
  // printf("t: %d btilde %d \n",t,beta2tilde[t]); /** here it is beta2tilde , it must be the same than alpha2 **/
}
/** Filtering over the horizon, **/
for (t=T0;t<100;t++)
{
    beta2[t]=beta2tilde[t];
    if(t<92) /** here we stop becaus we need the future and the index stop in 99 **/
    {


    for(k=0;k<8;k++) /** Horizon is 5+2**/
    {
        beta2[t]=MIN(beta2[t],beta2tilde[t+k]);
    }
    }
    else
    {
        beta2[t]=2;
    }

}
/** In order to compare beta2 et beta2tilde , uncomment
for (t=T0;t<99;t++)
{  printf("t: %d btilde %d ",t-T0,beta2tilde[t]);
  // beta2tilde[t]=xopt[1][t-2]-xopt[0][t]+2;
   printf("t: %d b %d \n",t-T0,beta2[t]);
}
**/
/** print of the optimal state, only x1 and x2 is affected of course **/
for(i=0;i<8;i++)
{


printf("x%d ",i);
for(t=T0;t<99;t++)
{
    if(xopt[i][t]<xopt[i][t+1]) printf(" + %d d^%d ",xopt[i][t],t-T0);

}


printf("+ %d^+oo\n",xopt[i][96]);
}

/** computation of  u2opt **/
for (t=94;t>=T0;t--)
{
    zeta[3][t]=z[1][t];
    zeta[2][t]=MAX(zeta[3][t+5],zeta[2][t+7]-beta2[t]);
    uopt[1][t]=zeta[2][t];

}

/** simulation with uopt and u2opt **/
/** Defintion of vector beta **/
int beta3tilde[100];
int beta3[100];
for (t=T0;t<94;t++)
{
    alpha1=2+xopt[1][t-2]-xopt[0][t-1]+xopt[3][t-2]-xopt[2][t-1]+xopt[5][t-2]-xopt[4][t-1]+xopt[7][t-2]-xopt[6][t-1];

    xopt[0][t]=Min(uopt[0][t],xopt[0][t-1]+alpha1);

    xopt[1][t]=xopt[0][t-3];

    alpha2=alpha1+xopt[0][t-1]-xopt[0][t];

    xopt[2][t]=Min(uopt[1][t],xopt[2][t-1]+alpha2);
    xopt[3][t]=xopt[2][t-5];
    alpha3=alpha2+xopt[2][t-1]-xopt[2][t];
    beta3tilde[t]=alpha2;
    xopt[4][t]=Min(uopt[2][t],xopt[4][t-1]+alpha3);
    xopt[5][t]=xopt[4][t-6];
    alpha4=alpha3+xopt[4][t-1]-xopt[4][t];
    xopt[6][t]=Min(uopt[3][t],xopt[6][t-1]+alpha4);
    xopt[7][t]=xopt[6][t-4];

}

for (t=T0;t<96;t++)
{ //printf("t: %d btilde %d ",t,beta3tilde[t]);  /** here it is alpaha2 which is printed actually **/
   beta3tilde[t]=xopt[1][t-2]-xopt[0][t]+xopt[3][t-2]-xopt[2][t]+2;
  // printf("t: %d btilde %d \n",t,beta3tilde[t]); /** here it is beta2tilde , it must be the same than alpha2 **/
}
/** Filtering over the horizon, **/
for (t=T0;t<100;t++)
{
    beta3[t]=beta3tilde[t];
    if(t<90) /** here we stop because we need the future and the index stop in 99 TAKE CARE ABOUT THIS **/
    {


    for(k=0;k<9;k++) /** Horizon is 5+2**/
    {
        beta3[t]=MIN(beta3[t],beta3tilde[t+k]);
    }
    }
    else
    {
        beta3[t]=2;
    }

}

/** print the optimal input fos S1 and S2 **/
for(i=0;i<4;i++)
{


printf("Uopt%d = ",i+1);
for(t=T0;t<99;t++)
{
    if(uopt[i][t]<uopt[i][t+1]) printf(" + %d d^%d ",uopt[i][t],t-T0);

}
printf("+ %d^+oo\n",uopt[i][96]);
}

/** print the optimal state with u1opt and u2 opt, to chack that x1 and x2 is unchanged **/
for(i=0;i<8;i++)
{


printf("x%d ",i);
for(t=T0;t<99;t++)
{
    if(xopt[i][t]<xopt[i][t+1])
        {
            printf(" + %d d^%d ",xopt[i][t],t-T0);
            //if(i==0) printf(" !! %d !",beta2[t+1]);
        }

}

printf("+ %d^+oo\n",xopt[i][93]);
}


/** computation of  u3opt **/
for (t=94;t>=T0;t--)
{
    zeta[5][t]=z[2][t];
    zeta[4][t]=MAX(zeta[5][t+6],zeta[4][t+8]-beta3[t]);
    uopt[2][t]=zeta[4][t];

}

/** simulation with uopt and u2opt **/
/** Defintion of vector beta **/
int beta4tilde[100];
int beta4[100];
for (t=T0;t<94;t++)
{
    alpha1=2+xopt[1][t-2]-xopt[0][t-1]+xopt[3][t-2]-xopt[2][t-1]+xopt[5][t-2]-xopt[4][t-1]+xopt[7][t-2]-xopt[6][t-1];

    xopt[0][t]=Min(uopt[0][t],xopt[0][t-1]+alpha1);

    xopt[1][t]=xopt[0][t-3];

    alpha2=alpha1+xopt[0][t-1]-xopt[0][t];

    xopt[2][t]=Min(uopt[1][t],xopt[2][t-1]+alpha2);
    xopt[3][t]=xopt[2][t-5];
    alpha3=alpha2+xopt[2][t-1]-xopt[2][t];
    beta4tilde[t]=alpha3;
    xopt[4][t]=Min(uopt[2][t],xopt[4][t-1]+alpha3);
    xopt[5][t]=xopt[4][t-6];
    alpha4=alpha3+xopt[4][t-1]-xopt[4][t];
    xopt[6][t]=Min(uopt[3][t],xopt[6][t-1]+alpha4);
    xopt[7][t]=xopt[6][t-4];

}

/** Filtering over the horizon, **/
for (t=T0;t<100;t++)
{
    beta4[t]=beta4tilde[t];
    if(t<91) /** here we stop becaus we need the future and the index stop in 99 **/
    {


    for(k=0;k<9;k++) /** Horizon is 5+2**/
    {
        beta4[t]=MIN(beta4[t],beta4tilde[t+k]);
    }
    }
    else
    {
        beta4[t]=2;
    }

}

/** print the optimal input fos S1 and S2 and S3 **/
for(i=0;i<4;i++)
{


printf("Uopt%d = ",i+1);
for(t=T0;t<99;t++)
{
    if(uopt[i][t]<uopt[i][t+1]) printf(" + %d d^%d ",uopt[i][t],t-T0);

}
printf("+ %d^+oo\n",uopt[i][96]);
}

/** print the optimal state with u1opt and u2 opt, to chack that x1 and x2 is unchanged **/
for(i=0;i<8;i++)
{


printf("x%d ",i);
for(t=T0;t<99;t++)
{
    if(xopt[i][t]<xopt[i][t+1])
        {
            printf(" + %d d^%d ",xopt[i][t],t-T0);
            //if(i==0) printf(" !! %d !",beta2[t+1]);
        }

}

printf("+ %d^+oo\n",xopt[i][93]);
}
return(0);
 }

  catch(mem_limite l)
 {
	 cout<<"Exception : too many coefficent in polynom "<<l.memoire<<endl;
	 return(1);
 }

 catch(taille_incorrecte obj)
 { // 0 : r non causal
   // 1 : tentative d'accès à un element d'une matrice avec un indice incorrect
   // 2 : matrice de taille incompatible pour oplus, inf, otimes, rfrac, lfrac
   // 3 : etoile de matrice carré uniquement
	 cout<<"Exception  "<<obj.erreur<<endl;
	 return(1);
 }
}
