#define BDMLIB
#include "../mat_checks.h"
#include "design/ctrlbase.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;
}