/*!
\file
\brief Multi-Estimator (developped for PMSM)
 
 */

#include "base/user_info.h"
#include "base/loggers.h"
#include "estim/kalman.h"
#include "estim/mixtures.h"
#include "pmsmDS.h"
#include "filters.h"
#include "base/datasources.h"
#include "simulator_zdenek/ekf_example/ekf_obj.h"

using namespace bdm;
#ifdef MEX
#include <itpp/itmex.h>
#include <mex/mex_BM.h>
#include <mex/mex_logger.h>
#include <mex/mex_datasource.h>
#include <mex/mex_function.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<2 ) mexErrMsgTxt ( "Usage:\n"
		                                "[Res,estimators,Res2]=estimator(system, estimators, experiment, logger)\n"
		                                "  system     = struct('class','datasource',...);  % Estimated system\n"
		                                "  estimators = {struct('class','estimator',...),  % Estimators\n"
		                                "                struct('class','estimator',...),...} \n"
		                                "  === optional ==="
		                                "  experiment = struct('ndat',10);                 % number of data in experiment, full length of finite datasources, 10 otherwise \n"
		                                "  logger     = struct('class','mexlogger');       % How to store results, default=mexlog, i.e. matlab structure\n\n"
		                                "Output:\n"
		                                "  Res          Matlab structure with logged results, \n"  
		                                "  estimators   Array of estimators updated with data \n"
		                                "  Res2         When logfull log_level is on, this structure is filled with structures of posterior densities\n\n"
		                                "see documentation of classes datasource, BM, and mexlogger and their offsprings in BDM." );

	RV::clear_all();
	//CONFIG
	UImxArray F;
	try {
		F.addGroup ( input[0],"system" );
		F.addList ( input[1],"estimators" );
		if ( n_input>2 ) {
			F.addGroup ( input[2],"experiment" );
		}
		if ( n_input>3 ) {
			F.addGroup ( input[3],"logger" );
		}
	} catch ( SettingException e ) {
		it_error ( "error: "+string ( e.getPath() ) );
	}

	//DBG
	F.writeFile ( "pmsm_estim.cfg" );

#else
int main ( int argc, char* argv[] ) {
	const char *fname;
	if ( argc>1 ) {fname = argv[1];	}
	else { cout << "Missing configuration file.\n Usage: \n $> estimator config_file.cfg"<<endl; abort(); }
	UIFile F ( fname );
#endif
	
	shared_ptr<logger> L = UI::build <logger>( F, "logger" );
	shared_ptr<DS>  pDS = UI::build<DS> ( F, "system" );
	Array<shared_ptr<BM> > Es;			// array of estimators
	UI::get( Es, F, "estimators" );
	int nE = Es.length();	//number of estimators
	int Ndat;				//number of data records
	F.lookupValue ( "experiment.ndat",Ndat );
			
	if ( !L ) {
	#ifdef MEX
			//mex logger has only from_setting constructor - we  have to fill it...
			L=new mexlog ( Ndat );
	#else
			L=new stdlog();
	#endif
		}
	pDS->log_register ( *L, "true" );
	string Ename;
	for (int i=0; i<nE; i++){
		try {
			UI::get ( Ename, F.getRoot()["estimators"][i], "name",UI::optional );
		} catch ( ...) {
			Ename="Est"+num2str ( i );
		}
		Es(i)->log_register(*L,Ename); // estimate
	}
	L->init();

	vec dt=zeros ( pDS->_drv()._dsize() );   //data variable
	Array<datalink*> Dls(nE); 
	Array<datalink*> Dlsc(nE); 
	for (int i=0; i<nE; i++){
		Dls(i)=new datalink( Es(i)->_yrv(),pDS->_drv() ); //datalink between a datasource and estimator
		Dlsc(i)=new datalink( Es(i)->_rvc(),pDS->_drv() ); //datalink between a datasource and estimator
	}
	
	// Main cycle
	for ( int tK=1;tK<Ndat;tK++ ) {
		// Data Source
		pDS->step();							// simulator step
		pDS->getdata ( dt );					// read data
		pDS->log_write ();
		
		// Estimators
		for (int i=0; i<nE; i++){
			Es(i)->bayes ( Dls(i)->pushdown ( dt ), Dlsc(i)->pushdown(dt) );		// update estimates

			Es(i)->log_write ();
		}
		// Regulators
		L->step();
	}

	L->finalize();
#ifdef MEX
	mexlog* mL=dynamic_cast<mexlog*> ( L.get() );

	if ( mL ) { // user wants output!!
		if ( n_output<1 ) mexErrMsgTxt ( "Wrong number of output variables!" );
		output[0] = mL->toCell();
		if (n_output>1){ // write estimators
			UImxArray Ests;
			UI::save(Es, Ests,"estimators");
			output[1]=UImxArray::create_mxArray(Ests);
		}
		if (n_output>2) {
			mL->_setting_conf().setAutoConvert(true);
			output[2]= UImxArray::create_mxArray(mL->_setting_conf().getRoot());
		}
	}
#endif

	for (int i=0; i<nE; i++){
		delete Dls(i);
		delete Dlsc(i);
	}
}