/*!
\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"


using namespace bdm;
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 );

	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, "estimator" );
	int nE = Es.length();	//number of estimators
	int Ndat;				//number of data records
	F.lookupValue ( "experiment.ndat",Ndat );
		
	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); 
	for (int i=0; i<nE; i++){
		Dls(i)=new datalink( Es(i)->_drv(),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 ) );		// update estimates

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

	L->finalize();

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