/*!
  \file
  \brief Controllers for linear Gaussian systems
  \author Vaclav Smidl.

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

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

#include "../base/bdmbase.h"
#include "ctrlbase.h"
#include "../estim/arx.h"
#include "../estim/kalman.h"
//#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h>

namespace bdm{

	//! Controller using ARX model for estimation and LQG designer for control
	class LQG_ARX : public Controller{
		protected:
		//! Internal ARX estimator
			shared_ptr<ARX> ar;
		//! Internal LQG designer
			LQG lq;
			//! Intermediate StateSpace model
			shared_ptr<StateFromARX> Stsp;
			//! AR predictor
			shared_ptr<mlnorm<chmat> > pred;
			//! datalink from rvc to ar.bayes
			datalink rvc2ar_y;
			//! datalink from rvc to ar.bayes
			datalink_buffered rvc2ar_cond;
			
			//! flag to use iterations spread in time (ist)
			bool ist;
			//! flag to use windsurfer approach
			bool windsurfer;
		public:
			//! Default constructor
			LQG_ARX(): ar(), lq() { }

			//! adaptation is to store arx estimates in stsp
			void adapt(const vec &data){
				ar->bayes(rvc2ar_y.pushdown(data), rvc2ar_cond.pushdown(data));
				rvc2ar_cond.store_data(data);
			}
			void redesign(){
				ar->ml_predictor_update(*pred);
				Stsp->update_from(*pred);
				if (windsurfer) {
					mat Ry=pred->_R();
					lq.set_control_Qy(inv(Ry));
				}
				if (!ist) {
					lq.initial_belmann();
				}
				lq.redesign();
			}
			vec ctrlaction(const vec &cond) const{
				//cond is xt + ut
				vec state=cond.left(Stsp->_A().rows());
				if (Stsp->_have_constant()){
					state(state.length()-1) = 1;
				}
				return lq.ctrlaction(state,cond.right(Stsp->_B().cols()));
			}
			//! 
	//! LQG is defined by quadratic loss function
	/*! \f[ L(y,u) = (y-y_{req})'Q_y (y-y_{req}) + (u-u_{req})' Q_u (u-u_{req}) \f]
			expected input
			\code
		{ class="LQG";
			arx = {class="ARX", ...} // internal arx model, see 
			Qy = ("matrix", ...); 
			Qu = ("matrix", ...); 
			yreq = [];               // requested output
			ureq = [];               // requested input value
		}
			\endcode
	 */
			void from_setting(const Setting &set){
				ar=UI::build<ARX> (set, "ARX",UI::compulsory);

				mat Qu;
				mat Qy;
				
				UI::get(Qu, set, "Qu", UI::compulsory);
				UI::get(Qy, set, "Qy", UI::compulsory);
		
				vec y_req;
				if (!UI::get(y_req, set, "yreq", UI::optional))
					y_req = zeros(ar->_yrv()._dsize());
				
				int horizon;
				UI::get(horizon, set, "horizon", UI::compulsory);
				lq.set_control_parameters(Qy,Qu,y_req,horizon);
				
				int wind;
				if (UI::get(wind, set, "windsurfer", UI::optional)){
					windsurfer=wind>0;
				} else {
					windsurfer=false;
				};
				int ist_;
				if (UI::get(ist_, set, "ist", UI::optional)){
					ist=ist_>0;
				} else {
					ist=false;
				};
				validate();
			}

		void validate() {
			// ar is valid
			pred = ar->ml_predictor<chmat>();
			Stsp = new StateFromARX;
			RV xrv;
			RV urvm; //old ut
			Stsp->connect_mlnorm(*pred, xrv, urvm);
			lq.set_system(Stsp);
			lq.validate();
			
			rvc =concat(xrv, urvm);
			rvc2ar_y.set_connection(ar->_yrv(), rvc);
			rvc2ar_cond.set_connection(ar->_rvc(), rvc);
			//datalink from ARX to rvc
		}
		void log_register (logger &L, const string &prefix ) {
			ar->log_register(L,prefix);
		}
		void log_write ( ) const { 
			ar->log_write();
		}
		
	};
	UIREGISTER(LQG_ARX);
} // namespace