/*!
  \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 );
		ar->ml_predictor_update ( *pred );
	}
	void redesign() {
		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;
		}
		vec tmp=lq.ctrlaction ( state, cond.right ( Stsp->_B().cols() ) );
		if (!std::isfinite(sum(tmp))) {
			cout << "infinite ctrl action";
		}
		return tmp;
	}
	//!
	//! 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();

		rv = urvm; // rv is not shifted to t+1!!
		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();
	}
	//! access function
	const ARX& _ar() {return *ar.get();}
	//! access function
	mlnorm<chmat>* _pred() {return pred.get();}
	

};
UIREGISTER ( LQG_ARX );
} // namespace