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

#endif
#include <time.h>
using namespace std;
#include <iostream>
#include <time.h>
//#include "ominus.cpp"
//#include "expand.cpp"

#include "..\src\gd.cpp"
#include "..\src\poly.cpp"
#include "..\src\serieodotinprogress.cpp"
#include "..\src\smatrix.cpp"
#include "..\src\tools.cpp"



double inline min(double a,double b)
{
		if(a>b) return b;
		else return a;
}


poly rresid( serie s, long T){
poly temp;
gd m, cr, tempmon;
int crossi=1000;
int crossj=1000;

for (int i=0; i<s.getp().getn(); i++)
{
    m = s.getp().getpol(i);
    if(m.getd()< T)
    {
        temp = oplus(temp,m);
    }

       if(m.getd() >= T)
    {
        crossi= min(i,crossi);
     //   std::cout << " crossi:" << std::endl << crossi<< std::endl;
    }
    if(crossi < 1000)
     {
     cr (s.getp().getpol(crossi).getg(),infinity);
         temp = oplus(temp,cr);
     }
}

int k=0;
int j=0;
m = s.getq().getpol(j);
gd r=s.getr();

while(m.getd()<T)
{  temp = oplus(temp,m);
    j++;
    if(j>s.getq().getn())
    {j=0; k++;
    r.init(r.getg()+s.getr().getg(),r.getd()+s.getr().getd());
    }
    m = otimes(s.getq().getpol(j),r);

}

    if(m.getd() >= T)
    {
        cr (m.getg(),infinity);
         temp = oplus(temp,cr);



   }



//std::cout << " rresid(u)" << std::endl << temp<< std::endl;
return temp;


}

poly ru( serie s, long T){
poly temp;
gd m, cr, tempmon;
int crossi=1000;
int crossj=1000;

for (int i=0; i<s.getp().getn(); i++)
{
    m = s.getp().getpol(i);
    if(m.getd()<= T)
    {
        temp = oplus(temp,m);
    }

       if(m.getd() > T)
    {
        crossi= min(i,crossi);
       // std::cout << " crossi:" << std::endl << crossi<< std::endl;
    }
    if(crossi < 1000)
     {
    cr (s.getp().getpol(crossi).getg(),T);
    //tempmon (infinity,infinity);
//cr=oplus(cr,tempmon);
     //  cr (infinity,T);

         temp = oplus(temp,cr);
     }
}

int k=0;
int j=0;
m = s.getq().getpol(j);
gd r=s.getr();

while(m.getd()<=T)
{  temp = oplus(temp,m);
    j++;
    if(j>s.getq().getn())
    {j=0; k++;
    r.init(r.getg()+s.getr().getg(),r.getd()+s.getr().getd());
    }
    m = otimes(s.getq().getpol(j),r);
}

    if(m.getd() > T)
    {
        cr (m.getg(),T);
         temp = oplus(temp,cr);



   }


// std::cout << " r(u)" << std::endl << temp<< std::endl;
return temp;


}
/////////////////////////////////////////////////////////////////////
/* Example :  Hardouin L., Cottenceau B., Lagrange S., Le Corronc E.

Performance a&nalysis of linear systems over idempotent semiring with additive inputs
Wodes 2008, Goteborg, Sweden
	*/
/////////////////////////////////////////////////////////////////////



int main()
{
    smatrix z1o,z2o, z1p, z2p,y1underline;
    long T=28;
	gd temp_gd, h1, h2;
	poly temp,SR_tf1,SR_tf2,temp1,temp2,x11,x12,x21,x22;
	serie temp_serie,temp_serie2,s1,s2,u1opts, u2opts;//,s3,s4;
	smatrix transfunctionH1,transfunctionH2,temp_matrix;
	int time_length;
	int index;
//	token_num* token_info;
//	token_num* window_info;
	int window_length;
    smatrix A1(2,2),B1(2,1),C1(1,2),AS1(2,2);
    smatrix A2(2,2),B2(2,1),C2(1,2),AS2(2,2);
    smatrix u1_star,x1_star;
    smatrix u2_star,x2_star,u1p_star,u2rcutm;

    smatrix z1(1,1);//,s3(1,1), s4(1,1);
    smatrix z2(1,1);
/////////////////////////////////////////////////////////////

    SR_tf1.init(1,1);
//////////
    h1.init(0,3);
    h2.init(0,4);


    ///* Test of Laurent ru and rresidu */
    temp1.init(0,10)(10,11)(20,30)(30,45)(60,infinity);
    temp.init(40,10)(61,11)(62,30)(73,45)(60,infinity);
    z1o(0,0).init(temp1,temp,e);
     cout<<"z1o"<<z1o<<endl;
    cout<<"temp"<<temp<<endl;
    cout<<"temp2"<<temp2<<endl;
    temp=ru(z1o(0,0),30);
    temp2=rresid(z1o(0,0),30);
    cout<<"z1o"<<z1o<<endl;
    cout<<"temp"<<temp<<endl;
    cout<<"temp2"<<temp2<<endl;
    fflush(stdin);getchar();
///* End test laurent */






     //temp.init(0,10)(1,11)(2,30)(3,45)(6,infinity);
     temp.init(0,10)(1,11)(2,50)(3,75)(6,infinity);
     z1(0,0).init(epsilon,temp,e);

////    rresid( z1(0,0), 25);
////    ru( z1(0,0), 25);
    temp.init(0,32)(1,38)(2,44)(5,infinity);
    z2(0,0).init(epsilon,temp,e);

    A1(0,1).init(temp.init(1,1),epsilon,e);
    A1(1,0).init(temp.init(0,3),epsilon,e);


    A2(0,1).init(temp.init(1,1),epsilon,e);
    A2(1,0).init(temp.init(0,4),epsilon,e);



      B1(0,0).init(epsilon,e,e);
      B2(0,0).init(epsilon,e,e);

     C1(0,1).init(epsilon,e,e);
    C2(0,1).init(epsilon,e,e);
////////////////////////////////////////////////////////////////
     AS1 = star(A1);
     AS2 = star(A2);
    temp_matrix = otimes(C1,AS1);
    transfunctionH1 = otimes(temp_matrix,B1);
    std::cout << " H1 :" << std::endl << transfunctionH1<< std::endl;
        u1_star = lfrac(z1,transfunctionH1); /** A **/
      //  u1p_star = lfrac(z1p,transfunctionH1);
    temp_matrix = otimes(C2,AS2);
    transfunctionH2 = otimes(temp_matrix,B2);
        std::cout << " H2 :" << std::endl << transfunctionH2<< std::endl;

    u2_star = lfrac(z2,transfunctionH2); /** B **/
    smatrix ASB1;
    ASB1 = otimes(AS1,B1);
    x1_star = otimes(ASB1,u1_star);
        std::cout<<"u1* : "<<std::endl<<u1_star<<std::endl;
        std::cout<<"x11* : "<<std::endl<<x1_star(0,0)<<std::endl;


    smatrix ASB2;
    ASB2 = otimes(AS2,B2);
    x2_star = otimes(ASB2,u2_star);
    std::cout<<"x2 star : "<<std::endl<<x2_star<<std::endl;
//
/////////////////////////////////////////////////////////////////
//
 poly z1poly,z2poly,z1pp;
 poly x21starpoly,x22starpoly,x21poly,x22poly;
 poly x11poly, x12poly,x11starpoly,x12starpoly;
 poly betah1,b3,G2;



//
// //   x23poly=frac(z2poly,H2);

// transformation as poly in order to use odot , odotfrac and odotflta
  x21starpoly=oplus(x2_star(0,0).getp(),x2_star(0,0).getq());
   x22starpoly=oplus(x2_star(1,0).getp(),x2_star(1,0).getq());
 x21poly=x21starpoly;
  x22poly=x22starpoly;
//
//
      x11starpoly=oplus(x1_star(0,0).getp(),x1_star(0,0).getq());
      x12starpoly=oplus(x1_star(1,0).getp(),x1_star(1,0).getq());
      x11poly=x11starpoly;
       x12poly=x12starpoly;


/** HERE COMPUTATION OF  optimal control for SYSTEM 2 **/
int i=0;
//
int step=1;
//
//  //   x4=z2;
//     //
//
      do{
   i++;

//cout << "i"<< i <<endl;

betah1= x21poly; // prevoius value of x21
b3=odot(x21poly,x11starpoly);
b3=frac(b3,SR_tf1); // Sr_tf1 is beta_1 actually

b3=fracodotsharp(b3,x12starpoly);

//
G2=oplus(A2(1,0).getp(),A2(1,0).getq());

b3=frac(b3,G2);


betah1=inf(x21poly,b3);
//cout << "betah1"<< betah1 <<endl;
if(!(x21poly==betah1)){
x21poly=betah1;
cout<<" step "<<step<<endl;
step ++;
}
else
step=21;


} while ( step <20);

if (step==21) cout<<" x21star !!!"<<x21poly<<endl;
    else cout<<"the optimal x21* doesn't exist"<<endl;
fflush(stdin); getchar();
/** HERE COMPUTATION OF  optimal state and output for the two systems **/


u1_star(0,0).init(x11poly,x11poly,e);
u2_star(0,0).init(x21poly,x21poly,e);
x1_star = otimes(ASB1,u1_star);
x2_star = otimes(ASB2,u2_star);


cout<<"x1_star"<<x1_star<<endl;
cout<<"x2_star"<<x2_star<<endl;
fflush(stdin);getchar();

    x11poly=oplus(x1_star(0,0).getp(),x1_star(0,0).getq());
    x12poly=oplus(x1_star(1,0).getp(),x1_star(1,0).getq());
    x21poly=oplus(x2_star(0,0).getp(),x2_star(0,0).getq());
    x22poly=oplus(x2_star(1,0).getp(),x2_star(1,0).getq());
poly check,checkR;
check=odot(x12poly,x22poly);
check=otimes(SR_tf1,check);
checkR=odot(x11poly,x21poly);
if(oplus(check,checkR)==checkR)
cout<<"The control is correct for the second system, contraints are satisfied \n";
else
cout<<"Oups with CDC paper \n";




T=19; /// Change of the reference input of z1, for example.

///* Computation of the smallest output y1 and y2 we can achieve *///
poly G1;
serie beta;
poly u1_cut,u1_min,u2_cut,u2_min,x11_min;
smatrix u2(1,1);
u1_cut=rresid(x11poly,T); /**  stop after T**/
u1_min=ru(x11poly,T);  /** full speed after T **/
cout<<"u1_cut"<<u1_cut<<endl;
cout<<"u1_min"<<u1_min<<endl;
int Gammaexpansion=40; // it must change according to the z1new given after
gd rp(Gammaexpansion,infinity);

u1_min=oplus(u1_min,rp);// to cut the series after Gammaexpansion events, it means full speed till Gammeexpansion events

cout<<"u1_min"<<u1_min<<endl;
u2_cut=rresid(x21poly,T); /**  stop after T**/
u2_min=ru(x21poly,T); /** full speed after T **/
cout<<"u2_cut"<<u2_cut<<endl;
cout<<"u2_min"<<u2_min<<endl;
u2(0,0).init(u2_cut,u2_cut,e);
x2_star=otimes(ASB2,u2); //here it is still a doubt may be x22poly is not that

//with poly
x21poly=oplus(x2_star(0,0).getp(),x2_star(0,0).getq());
x22poly=oplus(x2_star(1,0).getp(),x2_star(1,0).getq());
check=odot(x12poly,x22poly);
check=otimes(SR_tf1,check);
checkR=odot(x11poly,x21poly);
if(oplus(check,checkR)==checkR)
cout<<"POLY : The control is correct even when we cut the second, contraints are satisfied \n";
else
cout<<"Oups nightmare \n";




G1=oplus(AS1(1,0).getp(),AS1(1,0).getq());

x11_min=u1_min;
cout<<" u1_min "<<u1_min<<endl; getchar();


/*** HERE the forward algorithm to compute the x1_min compatible with the Rt#(x21)   ****/
/**** We need to use series since u1_min, but fracodotflat and odot of series is not still checked by Soraia ****/
poly p3,p2;
int stop=0;
p2=u1_min;
 do{
       p3=p2;

       p3=otimes(G1,p3);

     p3=odot(x22poly,p3);

p3=otimes(SR_tf1,p3);
p3=fracodotflat(p3,x21poly); // is only working for polynomial until now!!!!
p3=oplus(p3,u1_min);

cout<<" p3 "<<p3<<"stop"<<stop<<endl;
if(p3==p2 || stop>=Gammaexpansion)
{ cout<<" stop ="<<stop<<endl;
    stop=Gammaexpansion+1;

}
else
{   p2=p3;
    stop++;
}

getchar();
   }
while(stop<(Gammaexpansion+1));

cout<<" x11_min "<<p2<<endl;
//u1_star(0,0).init(s1.getp(),s1.getq(),s1.getr());
u1_star(0,0).init(p2,p2,e);
x1_star=otimes(ASB1,u1_star);  ///that is the best  you can do if you stop at time T
cout<<" y1 underline "<<x1_star(1,0)<<endl;



 x11poly=p2;
 x12poly=otimes(G1,p2);

check=odot(x12poly,x22poly);
check=otimes(SR_tf1,check);
checkR=odot(x11poly,x21poly);
if(oplus(check,checkR)==checkR)
cout<<"The control is correct for the first system, contraints are satisfied \n";
else
cout<<"Oups we are blocked \n";

getchar();


/*** We will consider this new y1 underline as z1  in order to check that it satisfies the backward constraint */

z1(0,0)=x1_star(1,0); ///** it is a series  so we will use the function odot odotfrac for series not completely tested and checked as Soria should did it, if problem we can use polynoimal function checked by Laurent
poly z1new=rresid(x1_star(1,0),80);
cout<<"z1new"<<z1new<<endl;
poly u1new=frac(z1new,G1);

cout<<"u1new"<<u1new<<endl;

stop=0;
x11poly=rresid(x1_star(0,0),T);
cout<<" x11poly "<<x11poly<<endl;
p3=inf(x11poly,u1new);
p3=u1new;
do{
p2=p3;
p2=odot(p2,x21poly);

p2=frac(p2,SR_tf1);
cout<<" p2 "<<p2<<endl;

p2=fracodotsharp(p2,x22poly);
cout<<" p2 "<<p2<<endl;
p2=frac(p2,G1);
p2=inf(p2,u1new );
cout<<" p2 "<<p2<<endl;
if(p2==p3 || stop>20)
{ cout<<"stop"<<stop<<endl;
    stop=21;

}
else
{   p3=p2;
    stop++;
}

getchar();
   }
while(stop<21);
cout<<" x11_min "<<p3<<endl;
cout<<" u1star(0,0) before 19 "<<u1_star(0,0)<<endl;

x11poly=p3;
 x12poly=otimes(G1,p3);

check=odot(x12poly,x22poly);
check=otimes(SR_tf1,check);
checkR=odot(x11poly,x21poly);
if(oplus(check,checkR)==checkR)
cout<<"The control is correct for the first system with the new control allowing to achieve Z1new \n";
else
cout<<"Oups we are not able to achieve z1new \n";

getchar();

}
