#define BDMLIB
#include "../mat_checks.h"
#include "design/lq_ctrl.h"

using namespace bdm;

TEST ( LQG_test ) {
    LQG reg;
    shared_ptr<StateSpace<chmat> > stsp = new StateSpace<chmat>;
    // 2 x 1 x 1
    stsp-> set_parameters ( eye ( 2 ), ones ( 2, 1 ), ones ( 1, 2 ), ones ( 1, 1 ), /* Q,R */ eye ( 2 ), eye ( 1 ) );
    reg.set_system ( stsp ); // A, B, C
    reg.set_control_parameters ( eye ( 1 ), eye ( 1 ),  vec_1 ( 1.0 ), 6 ); //Qy, Qu, horizon
    reg.validate();

    reg.redesign();
    double reg_apply = reg.ctrlaction ( "0.5, 1.1", "0.0" ) ( 0 ); /*convert vec to double*/
    CHECK_CLOSE ( reg_apply, -0.248528137234392, 0.0001 );
}

TEST ( to_state_test ) {
    mlnorm<fsqmat> ml;
    mat A = "1.1, 2.3";
    ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    ml.set_rv ( yr );
    yr.t_plus ( -1 );
    ml.set_rvc ( concat ( yr, ur ) );

    shared_ptr<StateCanonical > Stsp = new StateCanonical;
    Stsp->connect_mlnorm ( ml );

    /* results from
    [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
    */
    CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "2.53" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_D().get_row ( 0 ), vec ( "2.30" ), 0.0001 );
}

TEST ( to_state_arx_test ) {
    mlnorm<chmat> ml;
    mat A = "1.1, 2.3, 3.4";
    ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    ml.set_rv ( yr );
    ml.set_rvc ( concat ( yr.copy_t ( -1 ), concat ( ur, ur.copy_t ( -1 ) ) ) );

    shared_ptr<StateFromARX> Stsp = new StateFromARX;
    RV xrv;
    RV urv;
    Stsp->connect_mlnorm ( ml, xrv, urv );

    /* results from
    [A,B,C,D]=tf2ss([2.3 0],[1 -1.1])
    */
    CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1, 3.4, 1.3" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_B().get_row ( 0 ), vec ( "2.3" ), 0.0001 );
    CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "1, 0, 0" ), 0.0001 );
}

TEST ( arx_LQG_test ) {
    mlnorm<chmat> ml;
    mat A = "1.81, -.81, .00468, .00438";
    ml.set_parameters ( A, vec_1 ( 0.0 ), 0.00001*eye ( 1 ) );
    RV yr = RV ( "y", 1 );
    RV ur = RV ( "u", 1 );
    RV rgr = yr.copy_t ( -1 );
    rgr.add ( yr.copy_t ( -2 ) );
    rgr.add ( yr.copy_t ( -2 ) );
    rgr.add ( ur.copy_t ( -1 ) );
    rgr.add ( ur );

    ml.set_rv ( yr );
    ml.set_rvc ( rgr );
    ml.validate();

    shared_ptr<StateFromARX> Stsp = new StateFromARX;
    RV xrv;
    RV urv;
    Stsp->connect_mlnorm ( ml, xrv, urv );

    LQG L;
    L.set_system ( Stsp );
    L.set_control_parameters ( eye ( 1 ), sqrt ( 1.0 / 1000 ) *eye ( 1 ), vec_1 ( 0.0 ), 100 );
    L.validate();

    L.redesign();
    cout << L.to_string() << endl;
}

TEST (quadratic_test){
  quadraticfn qf;
  qf.Q = chmat(2);
  qf.Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0");
  CHECK_CLOSE_EX(qf.eval(vec("1 2")), vec_1(1.0), 0.0000000000000001);
  
  LQG_universal lq;
  lq.Losses = Array<quadraticfn>(1);
  lq.Losses(0) = quadraticfn();
  lq.Losses(0).Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0");
  lq.Losses(0).rv = RV("{u up }");
  
  lq.Models = Array<linfn>(1);
  lq.Models(0) = linfn(mat("1"),vec("1"));
  lq.Models(0).rv = RV("{x }");
  
  lq.rv = RV("u",1);
  
  lq.redesign();
  CHECK_CLOSE_EX(lq.ctrlaction(vec("1,0")), vec("1.24, -5.6"), 0.0000001);
}