/*!
  \file
  \brief Models for synchronous electric drive using IT++ and BDM
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/


#include <estim/ekf_templ.h>

//include dopravni model
#include "model.h"
#include "stat\libDS.h"
#include "stat\loggers.h"

using namespace bdm;

int main( int argc, char* argv[] ) {
/*	const char *fname;
	if ( argc>1 ) {fname = argv[1];	}
	else { fname = "k1.cfg"; }
	UIFile F ( fname ); //protected by exceptions, should complain if not found*/
	
	ITppFileDS DS("data.it","Data");		// Data Source
	int ndat = DS.ndat();				// number of data
	memlog L(ndat);						// Logger
	string resfile="k1_results.it";		// name of output file
	vec dQ="0.1 0.1";
	vec dR="0.2 0.2";					// TODO: read from config file <== broken on windows
	
	//model vyvoje stavu
	IMk1 fxu;
	fxu.set_parameters (0.5, 0.5); // pokud nejake budou

	//model pozorovani
	OMk1 hxu;
	//hxu.set_parameters (); // odkomentovat pokud budou

	// ESTIMATOR --- EKF
	// Priprava covariancnich matic pro EKF
	EKFCh Efix; 								//Extended KF s fix. variancemi
	Efix.set_parameters ( &fxu,&hxu,diag(dQ),diag(dR));
	Efix.set_statistics ( zeros(fxu.dimension()), 100*eye ( fxu.dimension() )  ); // nulova 

	// Definovat co se bude logovat
	Efix.log_add(L,"E");
	L.init(); // <<==== allocate memory for results
	
	vec dt;
	for ( int t=1;t<ndat;t++ ) {
		DS.getdata(dt);			// dt is allocated
		Efix.bayes(dt);			//ESTIMATE

		//LOG results
		Efix.logit(L); 
		
		L.step();
		DS.step();
	}
	L.finalize();
	L.itsave(resfile.c_str());
	return 0;
}