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


 */

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

#include <itpp/itmex.h>
#include <mex/mex_BM.h>
#include <mex/mex_logger.h>
#include <mex/mex_datasource.h>
#include <mex/mex_function.h>

using namespace bdm;

void mexFunction ( int n_output, mxArray *output[], int n_input, const mxArray *input[] ) {
	// Check the number of inputs and output arguments
	string fname;
	if (n_input>0){
		fname=mxArray2string(input[0]);
	} else {
		printf("file name missing");
	}
	// ------------------ 
	
	UIFile F ( fname.c_str());

	logger* L;
	DS * DS; 
	Array<BM*> Es;			// array of estimators
	int Ndat;				//number of data records
	int nE;					//number of estimators

	try {
		UIbuild ( F.lookup ( "logger" ),L );
		UIbuild ( F.lookup ( "system" ),DS );
		F.lookupValue ( "experiment.ndat",Ndat );
		Setting& S=F.lookup ( "estimator" );
		nE = S.getLength();
		Es.set_size(nE);
		for(int i=0;i<nE;i++){		
			UIbuild (S[i] ,Es(i) );
		}
	}
	catch UICATCH;

	DS->log_add ( *L );
	string nic="";
	for (int i=0; i<nE; i++){
		Es(i)->log_add(*L,nic); // estimate
	}
	L->init();

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

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

	L->finalize();

	// ------------------ End of routine -----------------------------
	
	mex_logger* mL=dynamic_cast<mex_logger*>(L);

	if (mL) { // user wants output!!
		if ( n_output<1 ) mexErrMsgTxt ( "Wrong number of output variables!" );
		output[0] = mL->toCell();
	}
	
	
	delete L;
	delete DS;
	for (int i=0; i<nE; i++){
		delete Dls(i);
		delete Es(i);
	}
}