#define BDMLIB
#include "UnitTest++.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);
}