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

using namespace bdm;

TEST(test_LQG) {
	LQG reg;
	shared_ptr<StateSpace<fsqmat> > stsp=new StateSpace<fsqmat>;
	// 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.redesign();
	double reg_apply=reg.apply("0.5, 1.1","0.0")(0); /*convert vec to double*/
	CHECK_CLOSE(reg_apply, -0.248528137234392, 0.0001);
}

TEST(test_to_state) {
	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);
}