#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"


using namespace std;

int main(void)
{
try{

smatrix a(3,3), as(3,3),b(3,1), c(1,3);
smatrix asb, casb,H1,H2,P1,P2,G1(2,2), G2(2,2), K0(2,1);
poly p;

srand(time(NULL));

// the system, matrix a
a(1,0)=gd(0,rand()%10); //
a(0,1)=gd(rand()%4+1,0); //
a(0,1)=gd(rand()%4+1,rand()%10);
a(1,1)=gd(rand()%4+1,rand()%10);
a(2,1)=gd(rand()%4+1,rand()%10);


// the system matrix b
b(0,0)=gd(0,0);


// the system matrix c
c(0,2)=gd(0,0);
//c(0,1)=gd(0,0);
//c(0,3)=gd(0,0);
//c(0,5)=gd(0,0);
//c(0,7)=gd(0,0);

// the star of the matrix a
as= star(a);

asb = otimes(as,b); // transfer matrix between input and state vector
H1 = otimes(c,asb); // trnasfer matrix between input and output

// the system, matrix a
a(1,0)=gd(0,rand()%10); //
a(0,1)=gd(rand()%4+1,0); //
a(0,1)=gd(rand()%4+1,rand()%10);
a(1,1)=gd(rand()%4+1,rand()%10);
a(2,1)=gd(rand()%4+1,rand()%10);


// the system matrix b
b(0,0)=gd(0,0);


// the system matrix c
c(0,2)=gd(0,0);
//c(0,1)=gd(0,0);
//c(0,3)=gd(0,0);
//c(0,5)=gd(0,0);
//c(0,7)=gd(0,0);
cout<<"Transfer of the system H1 \n"<<H1<<endl;
// the star of the matrix a
as= star(a);

asb = otimes(as,b); // transfer matrix between input and state vector
H2 = otimes(c,asb); // trnasfer matrix between input and output




cout<<"Transfer of the system H2 \n"<<H2<<endl;
smatrix s(1,1),s2(1,1);
s=oplus(H1,H2);
K0(0,0)=s(0,0);
s2=lfrac(s,H2);
K0(1,0)=s2(0,0);
s2=lfrac(s,H1);
K0(0,0)=s2(0,0);
G1(0,0)=H1(0,0);
G1(1,1)=H2(0,0);
G2(0,1)=H2(0,0);
G2(1,0)=H1(0,0);
int n;
smatrix K1,K;

n=0;
K1=prcaus(K0);
do
{   K0=K1;
    K=otimes(G1,K0);
    K=lfrac(K,G2);
    K1=otimes(G2,K0);
    K1=lfrac(K1,G1);
    K1=inf(K,K1);
    K1=inf(K1,K0);
    K1=prcaus(K1);
    n++;
}while(!(K1==K0) && n<20);

if (n<10)
{


cout<<"P1 : "<<K1(0,0)<<endl;
cout<<"P2 : "<<K1(1,0)<<endl;
}
else
{

cout<<" K0 "<<K0<<endl;
cout<<" K1 "<<K1<<endl;
cout<<" n "<<n<<endl;
}
smatrix Hmax,Htestmax;
smatrix H3;

s=K1(0,0);
s=prcaus(s);
Hmax=otimes(H1,s);
s2=K1(1,0);
s2=prcaus(s2);


Htestmax=otimes(H2,s2);
fflush(stdin);
getchar();

if (Htestmax==Hmax)
cout<<" until now it is ok, Hmax==H1P1==H2P2 "<<Hmax<<"\n";
cout<<" Hmax "<<Hmax<<endl;

/*smatrix Gref=star(Hmax);
cout<<"Gref" <<Gref<<endl;
smatrix P3=rfrac(Gref,Hmax);
cout<<"P3"<<P3<<endl;
smatrix G=otimes(Hmax,P3);
P3=prcaus(P3);
cout<<"P3 Caus"<<P3<<endl;
//smatrix G=otimes(Hmax,P3);
cout<<"G "<<G<<endl;
if(G==Gref) cout<<"ok"<<endl;

getchar();
*/

serie h_test;
 poly h_poly(13,13);
    h_poly.add(gd(20,15));
    gd h_r(35,23);

    h_test.init(epsilon,h_poly,h_r);
    Hmax(0,0)=h_test;
    cout<<" Hmax "<<Hmax<<endl;
smatrix Fopt(2,2);
s=lfrac(Hmax, Hmax);
s=rfrac(s,Hmax);
Fopt(0,0)=s(0,0);
Fopt(0,1)=s(0,0);

Fopt(1,0)=s(0,0);
Fopt(1,1)=s(0,0);
Fopt=prcaus(Fopt);
cout<<" Fopt : "<< Fopt<<endl;
smatrix GFSG;
smatrix G11(2,2);

G11(0,0)=Hmax(0,0);
G11(1,1)=Hmax(0,0);
GFSG=otimes(G11,Fopt);

GFSG=star(GFSG);
GFSG=otimes(GFSG,G11);

cout<<" (G1F)*G1 : "<< GFSG<<endl;

s=Fopt(0,0);
s=otimes(s,Hmax);
s=star(s);
s=otimes(Hmax,s);

cout<<" s "<<s<<endl;
smatrix Gr(2,2);
Gr(0,0)=s(0,0);
Gr(0,1)=s(0,0);
Gr(1,0)=s(0,0);
Gr(1,1)=s(0,0);

Fopt=lfrac(Gr,G11);
Fopt=rfrac(Fopt,G11);
Fopt=prcaus(Fopt);

cout<<" F "<<Fopt<<endl;
Fopt(0,0)=eps;
Fopt(1,1)=eps;
cout<<" (G11) : "<< G11<<endl;
GFSG=otimes(G11,Fopt);
cout<<" (G11Fopt) : "<< GFSG<<endl;
GFSG  =star(GFSG);
cout<<" (G1F)* : "<< GFSG<<endl;

serie Y2,Y1,Y11,Y12,Y21,Y22;
smatrix D1(1,1),D2(1,1);
D2(0,0)=gd(rand()%10,rand()%100);
D1(0,0)=gd(rand()%10,rand()%100);
Y22=otimes(GFSG(0,0),D2(0,0));
Y21=otimes(GFSG(0,1),D1(0,0));

Y12=otimes(GFSG(1,0),D2(0,0));
Y11=otimes(GFSG(1,1),D1(0,0));

Y2=oplus(Y22,Y21);
Y1=oplus(Y12,Y11);

cout<<" Y1\n "<<Y1<<endl;
cout<<" Y2\n "<<Y2<<endl;


GFSG=otimes(GFSG,G11);

cout<<" (G1F)*G1 : "<< GFSG<<endl;

smatrix V(2,1);
poly q;
gd r;
//p.init(rand()%10,rand()%10)(rand()%10,rand()%10);
//q.init(rand()%10,rand()%10)(rand()%10,rand()%10);
//r.init(rand()%10,rand()%10);
p.init(0,0);
q.init(5,50);
r.init(0,0);
V(0,0).init(p,q,r);
//p.init(rand()%10,rand()%10)(rand()%10,rand()%10);
//q.init(rand()%10,rand()%10)(rand()%10,rand()%10);
//r.init(rand()%10,rand()%10);
p.init(0,0);
q.init(0,0);
r.init(0,0);
V(1,0).init(p,q,r);
smatrix Y;
Y=otimes(GFSG,V);
cout<<" Y "<<Y<<endl;

//fstream f;

//  f.open("/home/brunsch/CAsB.txt",ios::out);
//f<<casb;
//f.close();

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