#include "ctrlbase.h"

namespace bdm {

void LQG::set_system ( shared_ptr<StateSpace<chmat> > S0 ) {
    S = S0;
}

void LQG::update_system() {
    pr.set_submatrix ( 0, 0, S->_B() );
    pr.set_submatrix ( 0, dimu, S->_A() );

    //penalization
    qux.set_submatrix ( 0, 0, Qu._Ch() );
    qux.set_submatrix ( 0, dimx + dimu + dimy, -Qu._Ch() );

    qyx.set_submatrix ( 0, 0, S->_C() );
    qyx.set_submatrix ( 0, dimx, -eye ( dimy ) );

    // parts of QR
    hqy = Qy.to_mat() * qyx * pr;

    // pre_qr
    pre_qr = concat_vertical ( s * pr, concat_vertical ( hqy, qux ) );
}

void LQG::set_control_parameters ( const mat &Qy0, const mat &Qu0, const vec &y_req0, int horizon0 ) {
    Qy = Qy0;
    Qu = Qu0;
    y_req = y_req0;
    horizon = horizon0;
}

void LQG::validate() {
    // set properties from S
    dimx = S->_A().rows();
    dimy = S->_C().rows();
    dimu = S->_B().cols();
    pr = zeros ( dimx + dimu + dimy,  dimu + dimx + dimu + dimy );
    pr.set_submatrix ( dimx, dimu + dimx, eye ( dimu + dimy ) );

    //penalization
    bdm_assert ( Qy.cols() == dimy, "LQG: wrong dimensions of Qy " );
    bdm_assert ( Qu.cols() == dimu, "LQG: wrong dimensions of Qu " );
    bdm_assert ( y_req.length() == dimy, "LQG: wrong dimensions of y_req " );

    qux = zeros ( dimu, dimx + 2 * dimu + dimy );
    qyx = zeros ( dimy, dimx + dimy + dimu );

    //
    initial_belmann();
    // parts of QR
    post_qr = zeros ( pre_qr.rows(), pre_qr.cols() );

    update_system();
}

void LQG::ricatti_step() {
//	pre_qr.set_submatrix ( 0, 0, s*pr );
//	pre_qr.set_submatrix ( dimx + dimu + dimy, dimu + dimx, -Qy.to_mat() *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 LQG::redesign() {
    for ( td = horizon; td > 0; td-- ) {
        update_system();
        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 );
}


}