#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;
}