#include "ctrlbase.h"

namespace bdm {

void LQG::set_system ( shared_ptr<StateSpace<chmat> > S0 ) {
	S = S0;
}

void LQG::update_system() {
	pr.set_submatrix ( 0, 0, S->_B() );
	pr.set_submatrix ( 0, dimu, S->_A() );

	//penalization
	qux.set_submatrix ( 0, 0, Qu._Ch() );
	qux.set_submatrix ( 0, dimx + dimu + dimy, -Qu._Ch() );

	qyx.set_submatrix ( 0, 0, S->_C() );
	qyx.set_submatrix ( 0, dimx, -eye ( dimy ) );

	// parts of QR
	hqy = Qy.to_mat() * qyx * pr;

	// pre_qr
	pre_qr = concat_vertical ( s * pr, concat_vertical ( hqy, qux ) );
}
	
void LQG::set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ) {
	Qy = Qy0;
	Qu = Qu0;
	y_req = y_req0;
	horizon = horizon0;
}

void LQG::validate() {
	// set properties from S
	dimx = S->_A().rows();
	dimy = S->_C().rows();
	dimu = S->_B().cols();
	pr = zeros ( dimx + dimu + dimy,  dimu + dimx + dimu + dimy );
	pr.set_submatrix ( dimx, dimu + dimx, eye ( dimu + dimy ) );

	//penalization
	bdm_assert ( Qy.cols() == dimy, "LQG: wrong dimensions of Qy " );
	bdm_assert ( Qu.cols() == dimu, "LQG: wrong dimensions of Qu " );
	bdm_assert ( y_req.length() == dimy, "LQG: wrong dimensions of y_req " );

	qux = zeros ( dimu, dimx + 2 * dimu + dimy );
	qyx = zeros ( dimy, dimx + dimy + dimu );

	//
	initial_belmann();
	// parts of QR
	post_qr = zeros ( pre_qr.rows(), pre_qr.cols() );

	update_system();
}

void LQG::ricatti_step() {
//	pre_qr.set_submatrix ( 0, 0, s*pr );
//	pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *y_req );
	if ( !qr ( pre_qr, post_qr ) ) {
		bdm_warning ( "QR in LQG unstable" );
	}
	triu ( post_qr );
	// hn(m+1:2*m+n+r,m+1:2*m+n+r);
	s = post_qr.get ( dimu, 2 * dimu + dimx + dimy - 1, dimu, 2 * dimu + dimx + dimy - 1 );
};

void LQG::redesign() {
	for ( td = horizon; td > 0; td-- ) {
		update_system();
		ricatti_step();
	}
	/*			ws=hn(1:m,m+1:2*m+n+r);
				wsd=hn(1:m,1:m);
				Lklq=-inv(wsd)*ws;*/
	L = -inv ( post_qr.get ( 0, dimu - 1, 0, dimu - 1 ) ) * post_qr.get ( 0, dimu - 1, dimu, 2 * dimu + dimx + dimy - 1 );
}


}
