/*!
  \file
  \brief Base classes for designers of control strategy
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/

#include "../base/bdmbase.h"
#include "../estim/kalman.h"

namespace bdm {

//! Base class for adaptive controllers
//! The base class is, however, non-adaptive, method \c adapt() is empty.
//! \note advanced Controllers will probably include estimator as their internal attribute (e.g. dual controllers)
class Controller : public root {
protected:
	//! identifier of the designed action;
	RV rv;
	//! identifier of the conditioning variables - data needed ;
	RV rvc;
public:
	//! function processing new observations and adapting control strategy accordingly
	virtual void adapt ( const vec &data ) {};
	//! function redesigning the control strategy
	virtual void redesign() {};
	//! returns designed control action
	virtual vec ctrlaction ( const vec &cond ) const {
		return vec ( 0 );
	}

	void from_setting ( const Setting &set ) {
		UI::get ( rv, set, "rv", UI::optional );
		UI::get ( rvc, set, "rvc", UI::optional );
	}
	//! access function
	const RV& _rv() {
		return rv;
	}
	//! access function
	const RV& _rvc() {
		return rvc;
	}
	//! register this controller with given datasource under name "name"
	virtual void log_register ( logger &L, const string &prefix ) { }
	//! write requested values into the logger
	virtual void log_write ( ) const { }

};

//! Linear Quadratic Gaussian designer for constant penalizations and constant target
//! Its internals are very close to Kalman estimator
class LQG : public Controller {
protected:
	//! StateSpace model from which we read data
	shared_ptr<StateSpace<chmat> > S;
	//! required value of the output y at time t (assumed constant)
	vec y_req;
	//! required value of the output y at time t (assumed constant)
	vec u_req;

	//! Control horizon, set to maxint for infinite horizons
	int horizon;
	//! penalization matrix Qy
	chmat Qy;
	//! penalization matrix Qu
	chmat Qu;
	//! time of the design step - from horizon->0
	int td;
	//! controller parameters
	mat L;

	//!@{ \name temporary storage for ricatti - use initialize
	//! convenience parameters
	int dimx;
	//! convenience parameters
	int dimy;
	//! convenience parameters
	int dimu;

	//!  parameters
	mat pr;
	//! penalization
	mat qux;
	//! penalization
	mat qyx;
	//! internal quadratic form
	mat s;
	//! penalization
	mat qy;
	//! pre_qr part
	mat hqy;
	//! pre qr matrix
	mat pre_qr;
	//! post qr matrix
	mat post_qr;
	//!@}

public:
	//! set system parameters from given matrices
	void set_system ( shared_ptr<StateSpace<chmat> > S0 );
	//! update internal whan system has changed
	void 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 ) );
	}
	//! set penalization matrices and control horizon
	void set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 );
	//! set penalization matrices and control horizon
	void set_control_Qy ( const mat &Qy0 ) {
		Qy = Qy0;
	}
	//! refresh temporary storage - inefficient can be improved
	void initial_belmann() {
		s = 1e-5 * eye ( dimx + dimu + dimy );
	};
	//! validation procedure
	void validate();
	//! function for future use which is called at each time td; Should call initialize()!
	//! redesign one step of the
	void 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 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 );
	}
	//! compute control action
	vec ctrlaction ( const vec &state, const vec &ukm ) const {
		vec pom = concat ( state, ones ( dimy ), ukm );
		return L*pom;
	}
	//! access function
	mat _L() const {
		return L;
	}
} ;


} // namespace