/*!
  \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 ) {
		shared_ptr<RV> rv_ptr = UI::build<RV>( set, "rv", UI::optional );
		if( rv_ptr ) rv = *rv_ptr;
		shared_ptr<RV> rvc_ptr = UI::build<RV>( set, "rvc", UI::optional );
		if( rvc_ptr ) rvc = *rvc_ptr;
	}
	//! 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();
	//! 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();
	
	void redesign();

	//! 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
