/*!
\file
\brief wrapper function for lqg.redesign()


 */

#include <design/arx_ctrl.h>

using namespace bdm;

#ifdef MEX
#include <mex/mex_parser.h>

void mexFunction ( int n_output, mxArray *output[], int n_input, const mxArray *input[] ) {
	// Check the number of inputs and output arguments
	if ( n_input<1 ) mexErrMsgTxt ( 
		" LQ controller of the given system according to criteria\n"
		"              L = y' Qy y + u' Qu u \n\n"
		"Usage:\n"
		"[vec,rv]=lqg_redesign(system,params)\n"
		"  system = struct('class','mlnorm',...);  % description of linear Gaussian system \n"
		"  params = struct('Qy',[], 'Qu',[], 'yr',[],'horizon', [1]);  \n"
		"output:\n"
		"  vec  vector of the controller.\n"
		"  rv   varibles in regressor of the controller.\n");

	RV::clear_all();
	//CONFIG
	if (!mxIsStruct(input[0])) mexErrMsgTxt("Given input is not a struct.");
	if ((n_input<2)||(!mxIsStruct(input[1]))) mexErrMsgTxt("Second parametr is not a structure.");
		
	UImxArray Cfg;
	Cfg.addGroup ( input[0],"system" );
	Cfg.addGroup ( input[1],"params" );
	Cfg.writeFile("lqg_redesign.cfg");

	// load stuff
	shared_ptr<mlnorm<chmat> > ml=UI::build<mlnorm<chmat> >(Cfg,"system");
	if (!ml) {mexErrMsgTxt("Incorrect input #1");}
	
	Setting &params=Cfg.getRoot()["params"];
	mat Qu;
	mat Qy;
	int horizon;
	vec yr;
	
	UI::get(Qy, params, "Qy",UI::compulsory);
	UI::get(Qu, params, "Qu",UI::compulsory);
	UI::get(yr, params, "yr",UI::compulsory);
	UI::get(horizon, params, "horizon", UI::compulsory);
	
				  // set it up
	shared_ptr<StateFromARX> Stsp=new StateFromARX;
	RV xrv;
	RV urv;
	Stsp->connect_mlnorm(*ml,xrv,urv);
	
	LQG lq;
	lq.set_system(Stsp);
	lq.set_control_parameters(Qy,Qu,yr,horizon);
	lq.validate();
	lq.redesign();
	
	mat L = lq._L();
	RV rv = lq._rvc();
	
	if ( n_output<1 ) mexErrMsgTxt ( "No output - nothing to do!" );
	output[0] = mxCreateDoubleMatrix(L.rows(),L.cols(), mxREAL);
	mat2mxArray(L, output[0]);
	if (n_output>1){

		UImxArray out;
		UI::save( &rv, out );
		output[1]= out.create_mxArray();
	}
		
}
#endif