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

#ifndef CTRL_BASE
#define CTRL_BASE


#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 {
  public:
    //! identifier of the designed action;
    RV rv;
protected:
    //! 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 ) {
        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 ) {
        vec pom = concat ( state, ones ( dimy ), ukm );
        return L*pom;
    }
    //! access function
    mat _L() const {
        return L;
    }
} ;


} // namespace

#endif //CTRL_BASE