/*!
  \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 <stat/libFN.h>
#include <estim/ekf_templ.h>
#include <stat/loggers.h>

//include dopravni model
#include "model.h"

using namespace bdm;

int main() {
	// Pocet dat
	int Ndat = 900;

	// Objekt pro ukladani vysledku
	memlog L(Ndat);

	//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
	vec mu0= "0.0 0.0 0.0 0.0";
	// Priprava covariancnich matic pro EKF
	vec Qdiag ( "1.0 10. 10 10" ); 
	vec Rdiag ( "1 1" ); //var(diff(xth)) = "0.034 0.034"
	mat Q =diag( Qdiag );
	mat R =diag ( Rdiag );
	EKFfull Efix ( RVstav,RVpozor,RVut );
	// pocatecni podminky
	Efix.set_est ( mu0, 1*eye ( 4 )  ); // nulova 
	// nastaveni modelu pro EKF
	Efix.set_parameters ( &fxu,&hxu,Q,R);

	int L_xt = L.add(RVstav, "xt"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat 
										// A jak se bude jsmenovat vysledek
	int L_ut = L.add(RVut, "ut"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat 
										// A jak se bude jsmenovat vysledek
	int L_mean = L.add(RVstav, "odh_xt"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat 
										// A jak se bude jsmenovat vysledek

	L.init(); // <<==== allocate memory for results
	// Priprava poli pro simulaci
	vec ut(RVut.count()); 
	vec xt(RVstav.count()); 
	vec dt(RVpozor.count()); 
	// minuly stav
	vec xtm=zeros(RVstav.count()); // nulovy pocatecni stav
	for ( int t=1;t<Ndat;t++ ) {
		// Nastaveni vstupu
		ut(0) = 1+sin((double)t/10); // V ut jsou same jednicky
		ut(1) = 1+cos((double)t/10); // V ut jsou same jednicky
		
		// Generovani DAT modelem
		xt = fxu.eval(xtm,ut);
		dt = hxu.eval(xt,ut);
		xtm = xt; //save xt for the next step

		//ESTIMATE
		Efix.bayes(concat(dt,ut));

		//LOG results
		L.logit(L_mean, Efix.posterior().mean() ); 
		L.logit(L_xt, xt ); 
		L.logit(L_ut, ut ); 
		
		L.step();
	}
	L.finalize();
	L.itsave("k1.it");
	return 0;
}