#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,H,H1,H2,P1,P2,G1(2,2), G2(2,2), K0(2,1);
smatrix ca2,ca1;
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);
ca1=otimes(c,as);
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()%100); //
a(0,1)=gd(rand()%40+1,0); //
a(0,1)=gd(rand()%40+1,rand()%100);
a(1,1)=gd(rand()%40+1,rand()%100);
a(2,1)=gd(rand()%40+1,rand()%100);


// 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);
ca2=otimes(c,as);
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)
{

P1=K1(0,0);
P2=K1(1,0);
cout<<"P1 : "<<K1(0,0)<<endl;
cout<<"P2 : "<<K1(1,0)<<endl;

H=otimes(H1,P1);
cout<<"H: "<<H<<endl;
H=otimes(H2,P2);
cout<<"H : "<<H<<endl;
smatrix F,F1,F2, HF1CA1,HF2CA2,HFCA2,HFCA1;
F=rfrac(H,H);
F=lfrac(F,H);
//F=prcaus(F);

cout<<"F : "<<F<<endl;
smatrix HF=otimes(H,F);
smatrix  HF2=otimes(HF,HF);


    cout<<" HFHF "<<HF2<< endl;
    cout<<" HF "<<HF<< endl;

smatrix HF2S=star(prcaus(HF2));
smatrix E(1,1);
E(0,0).init(epsilon,e,e);
smatrix EpHF2=oplus(E,HF2);
if(HF2S==EpHF2) cout<<" HF2S== E+HF2 "<<endl;
cout<<" HF2S "<<HF2S<< endl;
if( HF==HF2) cout<<" HF== HF2 !!"<<endl;
if(EpHF2==HF) cout<<" E+HF2== HF !! "<<endl;
else
{
    smatrix D=lfrac(EpHF2,HF);

    cout<<" D "<<D<<endl;
    smatrix shift=prcaus(D);
    cout<<" Shift "<<shift<<endl;
    smatrix d=lfrac(HF,EpHF2);
    cout<<" d "<<d<<endl;
    smatrix shift2=prcaus(d);
    cout<<" Shift2 "<<shift<<endl;
    gd val=shift(0,0).getp().getpol(0);
    cout<<" val "<<val<<endl;
     shift(0,0).init(epsilon,val,e);
      cout<<" Shift "<<shift<<endl;
}
smatrix HFHF2S=otimes(HF,HF2S);
if(HFHF2S==HF) cout<<" HFHF2S== HF "<<endl;
cout<<" HFHF2S "<<HFHF2S<< endl;
smatrix HFHF2Sca2=otimes(HFHF2S,ca2);
smatrix HF2Sca2=otimes(HF2S,ca2);


if(HF2Sca2==HFHF2Sca2) cout<<" HF2SCA2== HFHF2SCA2 "<<endl;

    cout<<" HF2Sca2 "<<HF2Sca2<< endl;
    cout<<" HFHF2Sca2 "<<HFHF2Sca2<< endl;

smatrix HFHF2Sca1=otimes(HFHF2S,ca1);
smatrix HF2Sca1=otimes(HF2S,ca1);


if(HF2Sca1==HFHF2Sca1) cout<<" HF2SCA1== HFHF2SCA1 "<<endl;

    cout<<" HF2Sca1 "<<HF2Sca1<< endl;
    cout<<" HFHF2Sca1 "<<HFHF2Sca1<< endl;
}

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