/*!
  \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{

//! Low-level class for design of control strategy
class Designer  {
	
	public:
		//! Redesign control strategy 
		virtual void redesign() {
			bdm_error("Not implemented");
		}

		//! apply control strategy to obtain control input
		virtual vec apply(const vec &cond) {
			bdm_error("Not implemented");
			return vec();
		}
};

//! Linear Quadratic Gaussian designer for constant penalizations and constant target
//! Its internals are very close to Kalman estimator
class LQG : public Designer {
	protected:
		//! StateSpace model from which we read data
		shared_ptr<StateSpace<fsqmat> > 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
		mat Qy;
		//! penalization matrix Qu
		mat 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<fsqmat> > S0);
		//! set penalization matrices and control horizon
		void set_control_parameters(const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0);
		//! set system parameters from Kalman filter
//		void set_system_parameters(const Kalman &K);
		//! refresh temporary storage - inefficient can be improved
		void initialize();
		//! validation procedure
		void validate();
		//! function for future use which is called at each time td; Should call initialize()!
		virtual void update_state(){};
		//! 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*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_state();
				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 apply(const vec &state, const vec &ukm){vec pom=concat(state, ones(dimy), ukm); return L*pom;}
	} ;

	//! 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 system output;
		RV yrv;
		//! identifier of the system input;
		RV urv;
		//! description of data needed for \c ctrlaction , required for automatic connection to DS
		RV drv;
		//! vector of logger IDs
		ivec LIDs;
		public:
			//! function processing new observations and adapting control strategy accordingly
			virtual void adapt(const vec &data){};
			//! returns designed control action
			virtual vec ctrlaction(const vec &data){return vec(0);}
			
			void from_setting(const Setting &set){
				UI::get(yrv,set,"yrv",UI::optional);
				UI::get(yrv,set,"urv",UI::optional);
			}
			//! access function
			const RV& _urv() {return urv;}
			//! access function
			const RV& _yrv() {return yrv;}
			//! access function
			const RV& _drv() {return drv;}
			//! register this controller with given datasource under name "name"
			virtual void log_add ( logger &L, const string &name = "" ) { }
			//! write requested values into the logger
			virtual void logit ( logger &L ) { }
			
	};
			
} // namespace