#define BDMLIB
#include "../mat_checks.h"
#include "design/lq_ctrl.h"

using namespace bdm;

TEST ( LQG_test ) {
    LQG reg;
    shared_ptr<StateSpace<chmat> > stsp = new StateSpace<chmat>;
    // 2 x 1 x 1
    stsp-> set_parameters ( eye ( 2 ), ones ( 2, 1 ), ones ( 1, 2 ), ones ( 1, 1 ), /* Q,R */ eye ( 2 ), eye ( 1 ) );
    reg.set_system ( stsp ); // A, B, C
    reg.set_control_parameters ( eye ( 1 ), eye ( 1 ),  vec_1 ( 1.0 ), 6 ); //Qy, Qu, horizon
    reg.validate();

    reg.redesign();
    double reg_apply = reg.ctrlaction ( "0.5, 1.1", "0.0" ) ( 0 ); /*convert vec to double*/
    CHECK_CLOSE ( reg_apply, -0.248528137234392, 0.0001 );
}

TEST ( to_state_test ) {
    mlnorm<fsqmat> ml;
    mat A = "1.1, 2.3";
    ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    ml.set_rv ( yr );
    yr.t_plus ( -1 );
    ml.set_rvc ( concat ( yr, ur ) );

    shared_ptr<StateCanonical > Stsp = new StateCanonical;
    Stsp->connect_mlnorm ( ml );

    /* results from
    [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
    */
    CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "2.53" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_D().get_row ( 0 ), vec ( "2.30" ), 0.0001 );
}

TEST ( to_state_arx_test ) {
    mlnorm<chmat> ml;
    mat A = "1.1, 2.3, 3.4";
    ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    ml.set_rv ( yr );
    ml.set_rvc ( concat ( yr.copy_t ( -1 ), concat ( ur, ur.copy_t ( -1 ) ) ) );

    shared_ptr<StateFromARX> Stsp = new StateFromARX;
    RV xrv;
    RV urv;
    Stsp->connect_mlnorm ( ml, xrv, urv );

    /* results from
    [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
    */
    CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1, 3.4, 1.3" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_B().get_row ( 0 ), vec ( "2.3" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "1, 0, 0" ), 0.0001 );
}

TEST ( arx_LQG_test ) {
    mlnorm<chmat> ml;
    mat A = "1.81, -.81, .00468, .00438";
    ml.set_parameters ( A, vec_1 ( 0.0 ), 0.00001*eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    RV rgr = yr.copy_t ( -1 );
    rgr.add ( yr.copy_t ( -2 ) );
    rgr.add ( yr.copy_t ( -2 ) );
    rgr.add ( ur.copy_t ( -1 ) );
    rgr.add ( ur );

    ml.set_rv ( yr );
    ml.set_rvc ( rgr );
    ml.validate();

    shared_ptr<StateFromARX> Stsp = new StateFromARX;
    RV xrv;
    RV urv;
    Stsp->connect_mlnorm ( ml, xrv, urv );

    LQG L;
    L.set_system ( Stsp );
    L.set_control_parameters ( eye ( 1 ), sqrt ( 1.0 / 1000 ) *eye ( 1 ), vec_1 ( 0.0 ), 100 );
    L.validate();

    L.redesign();
    cout << L.to_string() << endl;
}

TEST (LQGU_static_vs_Riccati_test){
  //test of universal LQG controller
  LQG_universal lq;
  lq.rv = RV("u", 2, 0);  
  lq.set_rvc(RV("x", 2, 0));
  lq.horizon = 10;

  /*
		model:      x = Ax + Bu			time: 0..horizon
		loss:       l = x'Q'Qx + u'R'Ru		time: 0..horizon-1
		final loss: l = x'S'Sx			time: horizon

		dim:	x: 2
				u: 2

		A = [	2	-1	 ]
			[	0	0.5	 ]
		
		B = [	1		-0.1	]	
			[	-0.2	2		]

		Q = [	5	0	]
			[	0	1	]

		R = [	0.01	0	 ]
			[	0		0.1	 ]

		S = Q
  */

  mat mA("2 -1;0 0.5"); 
  mat mB("1 -0.1;-0.2 2"); 
  mat mQ("5 0;0 1"); 
  mat mR("0.01 0;0 0.1"); 
  mat mS = mQ;

  //starting configuration
  vec x0("6 3");  	

  //model
  Array<linfnEx> model(2);
  //model Ax part
  model(0).A = mA;
  model(0).B = vec("0 0");
  model(0).rv = RV("x", 2, 0);
  model(0).rv_ret = RV("x", 2, 1);
  //model Bu part
  model(1).A = mB;
  model(1).B = vec("0 0");
  model(1).rv = RV("u", 2, 0);
  model(1).rv_ret = RV("x", 2, 1);	
  //setup
  lq.Models = model;

  //loss
  Array<quadraticfn> loss(2);
  //loss x'Qx part
  loss(0).Q.setCh(mQ);
  loss(0).rv = RV("x", 2, 1);
  //loss u'Ru part
  loss(1).Q.setCh(mR);
  loss(1).rv = RV("u", 2, 0);
  //setup
  lq.Losses = loss;

  //finalloss setup
  lq.finalLoss.Q.setCh(mS);
  lq.finalLoss.rv = RV("x", 2, 1);

  lq.validate();
    
  //produce last control matrix L
  lq.redesign();
  
  //verification via Riccati LQG version
  mat mK = mS;
  mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;
  
  //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion
  //more important is reached loss compared in the next part
  double tolerr = 1;//0.01; //0.01 OK x 0.001 NO OK

  //check last time L matrix
  CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr);
  
  mat oldK;
  int i;

  //produce next control matrix L
  for(i = 0; i < lq.horizon - 1; i++) {
	  lq.redesign();
	  oldK = mK;
	  mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ;
	  mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;

	  //check other times L matrix
	  CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr);
  }

 //check losses of LQG control - compare LQG_universal and Riccati version, no noise
    
  //loss of LQG_universal
  vec loss_uni("0");

  //setup
  vec x = x0;
  vec xold = x;
  vec u;
  //vec tmploss;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
	u = lq.getL().get_cols(0,1) * xold;
	x = mA * xold + mB * u;
	/*tmploss*/loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u;
	//loss_uni += tmploss.get(0);
	xold = x;
  }
  loss_uni = x.transpose() * mS * x;
  
  //loss of LQG Riccati version
  vec loss_rct("0");

  //setup
  x = x0;
  xold = x;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
	u = mL * xold;
	x = mA * xold + mB * u;
	loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;	
	xold = x;
  }
  loss_rct = x.transpose() * mS * x;  
  
  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);  
}

TEST (LQGU_random_vs_Riccati_test){
    
  //uniform random generator
  Uniform_RNG urng;
  double maxmult = 10.0;
  int maxxsize = 5;
  int maxusize = 5;

  urng.setup(2.0, maxxsize);  
  int matxsize = round_i(urng());
  urng.setup(2.0, maxusize);  
  int matusize = round_i(urng());

  urng.setup(-maxmult, maxmult);	

  mat tmpmat;
    
  mat mA;   
	mA = urng(matxsize, matxsize);	
  mat mB; 
	mB = urng(matxsize, matusize);	
  mat mQ; 
	tmpmat = urng(matxsize, matxsize);
	mQ = tmpmat*tmpmat.transpose();
  mat mR; 
	tmpmat = urng(matusize, matusize);
	mR = tmpmat*tmpmat.transpose();
  mat mS; 
	tmpmat = urng(matxsize, matxsize);
	mS = tmpmat*tmpmat.transpose();

  //starting configuration
  vec x0;
	x0 = urng(matxsize);	
 
  vec zerovecx(matxsize);
	zerovecx.zeros();
  vec zerovecu(matusize);
	zerovecu.zeros();

//test of universal LQG controller
  LQG_universal lq;
  lq.rv = RV("u", matusize, 0);  
  lq.set_rvc(RV("x", matxsize, 0));
  lq.horizon = 10;

  //model
  Array<linfnEx> model(2);
  //model Ax part
  model(0).A = mA;
  model(0).B = zerovecx;
  model(0).rv = RV("x", matxsize, 0);
  model(0).rv_ret = RV("x", matxsize, 1);
  //model Bu part
  model(1).A = mB;
  model(1).B = zerovecu;
  model(1).rv = RV("u", matusize, 0);
  model(1).rv_ret = RV("x", matxsize, 1);	
  //setup
  lq.Models = model;

  //loss
  Array<quadraticfn> loss(2);
  //loss x'Qx part
  loss(0).Q.setCh(mQ);
  loss(0).rv = RV("x", matxsize, 1);
  //loss u'Ru part
  loss(1).Q.setCh(mR);
  loss(1).rv = RV("u", matusize, 0);
  //setup
  lq.Losses = loss;

  //finalloss setup
  lq.finalLoss.Q.setCh(mS);
  lq.finalLoss.rv = RV("x", matxsize, 1);

  lq.validate();  

  //produce last control matrix L
  lq.redesign();
   
  //verification via Riccati LQG version
  mat mK = mS;
  mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;  

  //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion
  //more important is reached loss compared in the next part
  double tolerr = 2;

  //check last time L matrix
  CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr);
  
  mat oldK;
  int i;

  //produce next control matrix L
  for(i = 0; i < lq.horizon - 1; i++) {
	  lq.redesign();
	  oldK = mK;
	  mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ;
	  mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;	  

	  //check other times L matrix
	  CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr);
  }

 //check losses of LQG control - compare LQG_universal and Riccati version, no noise
    
  //loss of LQG_universal
  vec loss_uni("0");

  //setup
  vec x = x0;
  vec xold = x;
  vec u;
  //vec tmploss;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
	u = lq.getL().get_cols(0,(matxsize-1)) * xold;
	x = mA * xold + mB * u;
	loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u;	
	xold = x;
  }

  loss_uni = x.transpose() * mS * x;
  
  //loss of LQG Riccati version
  vec loss_rct("0");

  //setup
  x = x0;
  xold = x;

  //iteration
  for(i = 0; i < lq.horizon - 1; i++){
	u = mL * xold;
	x = mA * xold + mB * u;
	loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;	
	xold = x;
  }
  loss_rct = x.transpose() * mS * x;  
  
  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);  
}

TEST (LQGU_SiSy_test){
    int K = 5;
    int N = 100;    
    double sigma = 0.1;        
    double yr = 1;
	double y0 = 0;
    double x0 = y0 - yr;
	double Eb = 1;
	double P;
	//double P = 0.01;	//loss ~ 1.0???		e-2
	//double P = 0.1;	//loss ~ 1.???		e-1
	//double P = 1;		//loss ~ ???.???	e+2
	//double P = 10;	//loss ~ ?e+6
	       
    mat A("1"); 
    mat B(1,1);
		B(0,0) = Eb;
    mat Q("1");
    mat R("0");   
    
    vec u(K);
		u.zeros();
    mat xn(K, N);
		xn.zeros();
    vec S(K);            
		S.zeros();
    vec L(K);        
        L.zeros();

    vec loss(N); 
		loss.zeros();  

	LQG_universal lq;
	lq.rv = RV("u", 1, 0);  
	lq.set_rvc(RV("x", 1, 0));
	lq.horizon = K;

	Array<linfnEx> model(2);  
  model(0).A = A;
  model(0).B = vec("0 0");
  model(0).rv = RV("x", 1, 0);
  model(0).rv_ret = RV("x", 1, 1);  
  model(1).A = B;
  model(1).B = vec("0 0");
  model(1).rv = RV("u", 1, 0);
  model(1).rv_ret = RV("x", 1, 1);  
  lq.Models = model;

  Array<quadraticfn> qloss(2);  
  qloss(0).Q.setCh(Q);
  qloss(0).rv = RV("x", 1, 1);  
  qloss(1).Q.setCh(R);
  qloss(1).rv = RV("u", 1, 0);  
  lq.Losses = qloss;
  
  lq.finalLoss.Q.setCh(Q);
  lq.finalLoss.rv = RV("x", 1, 1);

  lq.validate();  
          
	int n, k;
	double Br;
	
	for(k = K-1; k >= 0;k--){
			lq.redesign();
			L(k) = (lq.getL())(0,0);
		}

for(P = 0.01; P <= 10; (P*=10)){
	lq.resetTime();	

	for(n = 0; n < N; n++){        
        Br = Eb + sqrt(P)*randn();        
   
                xn(0, n) = x0 + sigma*randn();            
				for(k = 0; k < K-1; k++){               
                   u(k) = L(k)*xn(k, n);                     
                   xn(k+1, n) = (A*xn(k, n))(0,0) + Br*u(k) + sigma*randn();                                       
				}
                
                loss(n) = 0;
				for(k=0;k<K;k++) loss(n) += xn(k, n)*xn(k, n);
                
	}
    
	double tolerr;
	//double P = 0.01;	//loss ~ 1.0???		e-2
	//double P = 0.1;	//loss ~ 1.???		e-1
	//double P = 1;		//loss ~ ???.???	e+2
	//double P = 10;	//loss ~ ?e+6
	if(P == 0.01) tolerr = 0.2;
	else if(P == 0.1) tolerr = 2;
	else if(P == 1) tolerr = 2000;
	else if(P == 10) tolerr = 2e+7;
	else tolerr = 0;

	CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr);
	}
}

TEST (LQGU_SiSy_SuSt_test){
	int K = 5;
    int N = 100;    
    double sigma = 0.1;        
    double yr = 1;
	double y0 = 0;    
	double Eb = 1;
	double inP;
	//double inP = 0.01;	//loss ~ 1.0???		e-2
	//double inP = 0.1;	//loss ~ 1.0???		e-2
	//double inP = 1;		//loss ~ 1.0???		e-2
	//double inP = 10;	//loss ~ 1.0???		e-2

	vec x0(3);
		x0(0) = y0 - yr;
		x0(1) = Eb;
			       
    mat A("1 0 0; 0 1 0; 0 0 1"); 
    mat B("0; 0; 0");
	mat X("1 0 0; 0 0 0; 0 0 0");	
	mat Y("0.00001");
    mat Q(3,3);
		Q.zeros();
		Q(1,1) = sigma*sigma;
        
    vec u(K);
		u.zeros();
	vec Kk(K);
		Kk.zeros();
    mat xn0(K+1, N);
		xn0.zeros();
	mat xn1(K+1, N);
		xn1.zeros();
	mat xn2(K+1, N);
		xn2.zeros();
    
	mat L(1, 3);
		L.zeros();
    vec loss(N); 
		loss.zeros();  

	LQG_universal lq;
	lq.rv = RV("u", 1, 0);  
	lq.set_rvc(RV("x", 3, 0));
	lq.horizon = K;

	Array<linfnEx> model(2);  
     model(0).A = A;
     model(0).B = vec("0 0");
     model(0).rv = RV("x", 3, 0);
     model(0).rv_ret = RV("x", 3, 1);  
     model(1).A = B;
     model(1).B = vec("0 0");
     model(1).rv = RV("u", 1, 0);
     model(1).rv_ret = RV("x", 3, 1);  
     lq.Models = model;

     Array<quadraticfn> qloss(2);  
     qloss(0).Q.setCh(X);
     qloss(0).rv = RV("x", 3, 1);  
     qloss(1).Q.setCh(Y);
     qloss(1).rv = RV("u", 1, 0);  
     lq.Losses = qloss;
  
     lq.finalLoss.Q.setCh(X);
     lq.finalLoss.rv = RV("x", 3, 1);

     lq.validate();  
          
	int n, k;
	
for(inP = 0.01; inP <= 10; (inP*=10)){
	lq.resetTime();
	x0(2) = inP;
	for(n = 0; n < N; n++){  
		L.zeros();
        xn0(0, n) = x0(0);           
		xn1(0, n) = x0(1) + sqrt(inP) * randn();            
		xn2(0, n) = x0(2); 
		//^neznalost, sum zatim ne

				for(k = 0; k < K-1; k++){               
                   u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n);
				   Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                   xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn();
                   xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k));
                   xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n);                                       
				}
                
				lq.resetTime();
				lq.redesign();
				for(k = K-1; k > 0; k--){               
					A(0, 1) = u(k);
                    A(1, 0) = -Kk(k);
                    A(1, 1) = 1-Kk(k)*u(k);
                    A(1, 2) = u(k)*sigma*sigma*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k)) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                    A(2, 2) = 1 - u(k)*u(k)*xn2(k, n)*(u(k)*u(k)*xn2(k, n) + 2*sigma*sigma) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                    B(0) = xn1(k, n);
                    B(1) = ((xn2(k, n)*sigma*sigma-u(k)*u(k)*xn2(k, n)*xn2(k, n))*(xn0(k+1, n)-xn0(k, n)) - 2*xn1(k, n)*u(k)) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                    B(2) = -2*u(k)*xn2(k, n)*xn2(k, n)*sigma*sigma / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
					lq.Models(0).A = A;
                    lq.Models(1).A = B;                                      
					lq.redesign();
				}
				L = lq.getL();

				//simulation
				xn0(0, n) += sigma*randn(); 
				for(k = 0; k < K-1; k++){
				   u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n);
				   Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma);
                   	   xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn();
                       xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k));
                       xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n);                                       
				}


                    loss(n) = 0;
				for(k=0;k<K;k++) loss(n) += xn0(k, n)*xn0(k, n);
                
	}	
    
	double tolerr = 0.2;	
	
	CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr);
	}
}

TEST (LQGU_PMSM_test){
	
	int K = 5;
	int Kt = 200;
	int N = 15;

	double a = 0.9898;
	double b = 0.0072;
	double c = 0.0361;
	double d = 1;
	double e = 0.0149; 

	double OMEGAt = 11.5;
	double DELTAt = 0.000125;

	double v = 0.0001;
	double w = 1;

	mat A(5,5);
	A.zeros();
	A(0,0) = a;
	A(1,1) = a;
	A(2,2) = d;
	A(3,3) = 1.0;
	A(4,4) = 1.0;
	A(2,4) = d-1.0;
	A(3,2) = DELTAt;
	A(3,4) = DELTAt;

	mat B(5,2);
	B.zeros();
	B(0,0) = c;
	B(1,1) = c;

	mat C(2,5);
	C.zeros();
	C(0,0) = c;
	C(1,1) = c;
	 
	mat X(5,5);
	X.zeros();
	X(2,2) = w;

	mat Y(2,2);
	Y.zeros();
	Y(0,0) = v;
	Y(1,1) = v;

	mat Q(5,5);
	Q.zeros();
	Q(0,0) = 0.0013;
	Q(1,1) = 0.0013;
	Q(2,2) = 5e-6;
	Q(3,3) = 1e-10;

	mat R(2,2);
	R.zeros();
	R(0,0) = 0.0006;
	R(0,0) = 0.0006;

	vec x0(5);
	x0.zeros();
	x0(2) = 1.0-OMEGAt;
	x0(3) = itpp::pi / 2;
	x0(4) = OMEGAt;

	mat P(5,5);
	P.zeros();
	P(0,0) = 0.01;
	P(1,1) = 0.01;
	P(2,2) = 0.01;
	P(3,3) = 0.01;
	
	vec u(2);
	
	mat x(5,Kt+K);	

	mat L(2, 5);
	mat L0(5, Kt);
	mat L1(5, Kt); 

	int i,n,k,kt;
	vec x00(5);
	vec randvec(2);

	LQG_universal lq;
	lq.rv = RV("u", 2, 0);  
	lq.set_rvc(RV("x", 5, 0));
	lq.horizon = K;

	Array<linfnEx> model(2);  
	model(0).A = A;
	model(0).B = vec("0 0 0 0 0");
	model(0).rv = RV("x", 5, 0);
	model(0).rv_ret = RV("x", 5, 1);  
	model(1).A = B;
	model(1).B = vec("0 0");
	model(1).rv = RV("u", 2, 0);
	model(1).rv_ret = RV("x", 5, 1);  
	lq.Models = model;

	Array<quadraticfn> qloss(2);  
	qloss(0).Q.setCh(X);
	qloss(0).rv = RV("x", 5, 1);  
	qloss(1).Q.setCh(Y);
	qloss(1).rv = RV("u", 2, 0);  
	lq.Losses = qloss;
  
	lq.finalLoss.Q.setCh(X);
	lq.finalLoss.rv = RV("x", 5, 1);

	lq.validate(); 

	vec losses(N);
		losses.zeros();
	vec omega(Kt+K);
		omega.zeros();
	
	for(n = 0; n < N; n++) {
		L0.zeros();
		L1.zeros();
						    
		for(i=0;i<5;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn();   
		       
		for(kt = 0; kt < Kt; kt++){            
			x.set_col(0, x00);

			for(k = 0; k < kt+K-1; k++){
				L.set_size(2,5);
				L.set_row(0, L0.get_col(kt));
				L.set_row(1, L1.get_col(kt));
				u = L*x.get_col(k);
				if(u(0) > 50) u(0) = 50;
				else if(u(0) < -50) u(0) = -50;
				if(u(1) > 50) u(1) = 50;
				else if(u(1) < -50) u(1) = -50;
            
				x(0, k+1) = a*x(0, k) + b*(x(2, k) + x(4, k))*sin(x(3, k)) + c*u(0);  
				x(1, k+1) = a*x(1, k) - b*(x(2, k) + x(4, k))*cos(x(3, k)) + c*u(1);
				x(2, k+1) = d*x(2, k) + (d-1)*x(4, k) + e*(x(1, k)*cos(x(3, k)) - x(0, k)*sin(x(3, k)));
				x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt;
				x(4, k+1) = x(4, k); 				
			}
                        
            lq.resetTime();
			lq.redesign();
            for(k = K-1; k > 0; k--){               
                A(2, 0) = -e*sin(x(3, k+kt-1));
                A(2, 1) = e*cos(x(3, k+kt-1));
                A(0, 2) = b*sin(x(3, k+kt-1));
                A(1, 2) = -b*cos(x(3, k+kt-1));
                A(0, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*cos(x(3, k+kt-1));
                A(1, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*sin(x(3, k+kt-1));    
                A(2, 3) = -e*(x(1, k+kt-1)*sin(x(3, k+kt-1) + x(0,k+kt-1)*cos(x(3, k+kt-1))));
                A(0, 4) = b*sin(x(3, k+kt-1));
                A(1, 4) = -b*cos(x(3, k+kt-1));                                         
				lq.Models(0).A = A;                                                      
				lq.redesign();
			}
			L = lq.getL();
			L0.set_col(kt, L.get_row(0).get(0,4));
			L1.set_col(kt, L.get_row(1).get(0,4));
		}        
       
        x.set_col(0, x00);
		
        for(k = 0; k < Kt; k++){ 
	
			L.set_size(2,5);
			L.set_row(0, L0.get_col(k));
			L.set_row(1, L1.get_col(k));
			u = L*x.get_col(k);     
			if(u(0) > 50) u(0) = 50;
			else if(u(0) < -50) u(0) = -50;
			if(u(1) > 50) u(1) = 50;
			else if(u(1) < -50) u(1) = -50;
				
            x(0, k+1) = a*x(0, k) + b*(x(2, k) + x(4, k))*sin(x(3, k)) + c*u(0) + sqrt(Q(0, 0))*randn();  
			x(1, k+1) = a*x(1, k) - b*(x(2, k) + x(4, k))*cos(x(3, k)) + c*u(1) + sqrt(Q(1, 1))*randn();
			x(2, k+1) = d*x(2, k) + (d-1)*x(4, k) + e*(x(1, k)*cos(x(3, k)) - x(0, k)*sin(x(3, k))) + sqrt(Q(2, 2))*randn();
			x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt + sqrt(Q(3, 3))*randn(); 
			x(4, k+1) = x(4, k); 
	
			losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0); 			
		}

		omega += x.get_row(2);
	}

	cout << "Loss: " << sum(losses)/N << endl;
}

TEST (LQGU_181_zero){
	/*						 v ????? .8189
		y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1}

		S = y^2 + u^2/1000

		y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1}
		y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581
	*/

	int K = 150;

	LQG_universal lq;
	lq.rv = RV("ut", 1, 0); 
	RV rvc;	
	rvc = RV("yt", 1, -1);	
	rvc.add(RV("yt", 1, -2));
	rvc.add(RV("ut", 1, -1));	
	rvc.add(RV("1", 1, 0));
	lq.set_rvc(rvc);
	lq.horizon = K;

	Array<linfnEx> model(4);  
	model(0).A = mat("1.81");
	model(0).B = vec("0");
	model(0).rv = RV("yt", 1, -1);
	model(0).rv_ret = RV("yt", 1, 0);	 
	model(1).A = mat("-0.8189");
	model(1).B = vec("0");
	model(1).rv = RV("yt", 1, -2);
	model(1).rv_ret = RV("yt", 1, 0);	
	model(2).A = mat("0.00438");
	model(2).B = vec("0");
	model(2).rv = RV("ut", 1, 0);
	model(2).rv_ret = RV("yt", 1, 0);	
	model(3).A = mat("0.00468");
	model(3).B = vec("0");
	model(3).rv = RV("ut", 1, -1);
	model(3).rv_ret = RV("yt", 1, 0);	
	lq.Models = model;

	Array<quadraticfn> qloss(2);  
	qloss(0).Q.setCh(mat("1"));
	qloss(0).rv = RV("yt", 1, 0);	
	qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)	
	qloss(1).rv = RV("ut", 1, 0); 	
	lq.Losses = qloss;
 
	lq.finalLoss.Q.setCh(mat("1"));
	lq.finalLoss.rv = RV("yt", 1, 0);	

	lq.validate(); 

	lq.resetTime();
	lq.redesign();

	for(int k = K-1; k > 0; k--){		
		lq.redesign();
	}

	mat resref("-69.4086   47.2578   -0.2701   0.0");
	CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
}

TEST (LQGU_181_ten){
	/*						 v ????? .8189
		y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1}

		S = y^2 + u^2/1000

		y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1}
		y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581
	*/

	int K = 150;
	
	LQG_universal lq;
	lq.rv = RV("ut", 1, 0); 
	RV rvc;	
	rvc = RV("yt", 1, -1);	
	rvc.add(RV("yt", 1, -2));
	rvc.add(RV("ut", 1, -1));	
	rvc.add(RV("1", 1, 0));
	lq.set_rvc(rvc);
	lq.horizon = K;

	Array<linfnEx> model(4);  
	model(0).A = mat("1.81");
	model(0).B = vec("0");
	model(0).rv = RV("yt", 1, -1);
	model(0).rv_ret = RV("yt", 1, 0);	 
	model(1).A = mat("-0.8189");
	model(1).B = vec("0");
	model(1).rv = RV("yt", 1, -2);
	model(1).rv_ret = RV("yt", 1, 0);	
	model(2).A = mat("0.00438");
	model(2).B = vec("0");
	model(2).rv = RV("ut", 1, 0);
	model(2).rv_ret = RV("yt", 1, 0);	
	model(3).A = mat("0.00468");
	model(3).B = vec("0");
	model(3).rv = RV("ut", 1, -1);
	model(3).rv_ret = RV("yt", 1, 0);	
	lq.Models = model;

	Array<quadraticfn> qloss(2);  
	qloss(0).Q.setCh(mat("1 -10"));
	qloss(0).rv = RV("yt", 1, 0);	
	qloss(0).rv.add(RV("1", 1, 0));
	qloss(1).Q.setCh(mat("0.03162")); 	
	qloss(1).rv = RV("ut", 1, 0); 	
	lq.Losses = qloss;
 
	lq.finalLoss.Q.setCh(mat("0"));
	lq.finalLoss.rv = RV("yt", 1, 0);	

	lq.validate(); 

	lq.resetTime();
	lq.redesign();

	for(int k = K-1; k > 0; k--){		
		lq.redesign();
	}

	mat resref("-69.4086   47.2578   -0.2701   233.758");
	CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
}

TEST (LQGU_181_reqv){
	/*						 v ????? .8189
		y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1}

		S = y^2 + u^2/1000

		y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1}
		y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581
	*/

	int K = 200;	

	LQG_universal lq;
	lq.rv = RV("ut", 1, 0); 
	RV rvc;	
	rvc = RV("yt", 1, -1);	
	rvc.add(RV("yt", 1, -2));
	rvc.add(RV("ut", 1, -1));
	rvc.add(RV("yr", 1, 0));
	rvc.add(RV("1", 1, 0));
	lq.set_rvc(rvc);
	lq.horizon = K;

	Array<linfnEx> model(5);  
	model(0).A = mat("1.81");
	model(0).B = vec("0");
	model(0).rv = RV("yt", 1, -1);
	model(0).rv_ret = RV("yt", 1, 0);	 
	model(1).A = mat("-0.8189");
	model(1).B = vec("0");
	model(1).rv = RV("yt", 1, -2);
	model(1).rv_ret = RV("yt", 1, 0);	
	model(2).A = mat("0.00438");
	model(2).B = vec("0");
	model(2).rv = RV("ut", 1, 0);
	model(2).rv_ret = RV("yt", 1, 0);	
	model(3).A = mat("0.00468");
	model(3).B = vec("0");
	model(3).rv = RV("ut", 1, -1);
	model(3).rv_ret = RV("yt", 1, 0);	
	model(4).A = mat("1");
	model(4).B = vec("0");
	model(4).rv = RV("yr", 1, 0);
	model(4).rv_ret = RV("yr", 1, 1);
	lq.Models = model;

	Array<quadraticfn> qloss(2);  
	qloss(0).Q.setCh(mat("1 -1"));
	qloss(0).rv = RV("yt", 1, 0);
    	qloss(0).rv.add(RV("yr", 1, 0));
	qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)	
	qloss(1).rv = RV("ut", 1, 0); 	
	lq.Losses = qloss;
 
	lq.finalLoss.Q.setCh(mat("1 -1"));
	lq.finalLoss.rv = RV("yt", 1, 0);	
	lq.finalLoss.rv.add(RV("yr", 1, 0));	

	lq.validate(); 

	lq.resetTime();
	lq.redesign();

	for(int k = K-1; k > 0; k--){		
		lq.redesign();
	}

	mat resref("-69.4086   47.2578   -0.2701   23.3758   0.0");
	CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
}

TEST (LQGU_Filatov){
	/*						 
		system from:
		Adaptive Predictive Control Policy for Nonlinear Stochastic Systems
		(N. M. Filatov, H. Unbehauen)
		VII. Simulations

		q(k+1) = 1.654q(k) - 1.022q(k-1) + 0.2019q(k-2)
			+0.1098u(k) + 0.0792u(k-1) - 0.0229u(k-2)
		J(k) = (w(k+1) - q(k+1))^2 + 0.0005(u(k))^2
		w(k) == 5
		K = 50
		q(>=0) = 0.1;

		! used without noise
	*/

	int K = 50;
	
	LQG_universal lq;
	lq.rv = RV("u", 1, 0); 
	RV rvc;	
	rvc = RV("q", 1, 0);	
	rvc.add(RV("q", 1, -1));
	rvc.add(RV("q", 1, -2));	
	rvc.add(RV("u", 1, -1));
	rvc.add(RV("u", 1, -2));
	rvc.add(RV("w", 1, 0));
	rvc.add(RV("1", 1, 0));
	lq.set_rvc(rvc);
	lq.horizon = K;

	Array<linfnEx> model(7);  
	model(0).A = mat("1.654");
	model(0).B = vec("0");
	model(0).rv = RV("q", 1, 0);
	model(0).rv_ret = RV("q", 1, 1);	 
	model(1).A = mat("-1.022");
	model(1).B = vec("0");
	model(1).rv = RV("q", 1, -1);
	model(1).rv_ret = RV("q", 1, 1);	
	model(2).A = mat("0.2019");
	model(2).B = vec("0");
	model(2).rv = RV("q", 1, -2);
	model(2).rv_ret = RV("q", 1, 1);	
	model(3).A = mat("0.1098");
	model(3).B = vec("0");
	model(3).rv = RV("u", 1, 0);
	model(3).rv_ret = RV("q", 1, 1);	
	model(4).A = mat("0.0792");
	model(4).B = vec("0");
	model(4).rv = RV("u", 1, -1);
	model(4).rv_ret = RV("q", 1, 1);
	model(5).A = mat("-0.0229");
	model(5).B = vec("0");
	model(5).rv = RV("u", 1, -2);
	model(5).rv_ret = RV("q", 1, 1);	
	model(6).A = mat("1");
	model(6).B = vec("0");
	model(6).rv = RV("w", 1, 0);
	model(6).rv_ret = RV("w", 1, 1);	
	lq.Models = model;

	Array<quadraticfn> qloss(2);  
	qloss(0).Q.setCh(mat("1 -1"));
	qloss(0).rv = RV("q", 1, 1);
    	qloss(0).rv.add(RV("w", 1, 1));	
	qloss(1).Q = mat("0.0005");//automatic sqrt
	qloss(1).rv = RV("u", 1, 0); 	
	lq.Losses = qloss;
 
	lq.finalLoss.Q.setCh(mat("1 -1"));
	lq.finalLoss.rv = RV("q", 1, 1);	
	lq.finalLoss.rv.add(RV("w", 1, 1));	

	lq.validate(); 

	lq.resetTime();
	lq.redesign();

	int k;

	for(k = K-1; k > 0; k--) lq.redesign();
	
	mat resref("-12.1148   7.9949   -1.6191   -0.6123   0.1836   7.1642 0.0");
	CHECK_CLOSE_EX(resref, lq.getL(), 0.01);	
}

TEST (Matrix_Division){	
	Uniform_RNG urng;

	mat A("1 1 1 1 1; 0 2 2 2 2; 0 0 3 3 3; 0 0 0 4 4; 0 0 0 0 5");
	mat B = urng(5,5);		
	mat C1, C2;

	C1 = inv(A)*B;	
	ldutg(A,B,C2);	
		
	CHECK_CLOSE_EX(C1, C2, 0.000001);	
}