/*!
  \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 <bdmbase.h>

//! Base class of designers of control strategy
class Designer : public root {
	public:
		//! Redesign control strategy 
		virtual redesign(){it_error("Not implemented"); };
		//! apply control strategy to obtain control input
		virtual vec apply(const vec &cond){it_error("Not implemented"); return vec(0);}
}

//! Linear Quadratic Gaussian designer for constant penalizations and constant target
//! Its internals are very close to Kalman estimator
class LQG : public Designer {
	protected:
		//! dimension of state
		int dimx;
		//! dimension of inputs
		int dimu;
		//! dimension of output
		/int dimy;

		//! matrix A of the linear system
		mat A;
		//! matrix B of the linear system
		mat B;
		//! matrix C of the linear system
		mat C;
		//! expected value of x at time t
		vec xt;
		//! required value of the output y at time t (assumed constant)
		vec y_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
		//!  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_parameters(const mat &A, const mat &B, const mat &C);
		//! set system parameters from Kalman filter
		void set_system_parameters(const Kalman &K);
		//! set current state
		void set_state(const vec &xt0){xt=xt0;};
		//! refresh temporary storage 
		//! function for future use which is called at each time td;
		virtual update_state(){};
		//! redesign one step of the 
		void ricatti_step(){
			pre_qr.set_submatrix(0,0,s*pr);
			pre_qr.set_submatrix(dimx, dimu+dimx, -Qy*y_req);
			post_qr=qr(pre_qr);
			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, dimu, 2*dimu+dimx+dimy);
		};
		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, dimu, 2*dimu+dimx+dimy);
		}
		vec apply(const vec &state, const vec &ukm){vec pom=concat_vertical(state, ones(dimy,1), ukm); return L*pom;}
}