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

#include "pmsm.h"
#include "simulator.h"
#include "sim_profiles.h"

#include <stat/loggers.h>

using namespace itpp;

int main() {
	// Kalman filter
	int Ndat = 90000;
	double h = 1e-6;
	int Nsimstep = 125;
	int Npar = 10;

	dirfilelog L("exp/pmsm_mix",1000);
	//memlog L(Ndat);
	
	// SET SIMULATOR
	pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
	double Ww = 0.0;
	vec dt ( 2 );
	vec ut ( 2 );
	vec xtm=zeros ( 4 );
	vec xdif=zeros ( 4 );
	vec xt ( 4 );
	vec ddif=zeros(2);
	IMpmsm fxu;
	//                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz)
	fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
	OMpmsm hxu;
	mat Qt=zeros ( 4,4 );
	mat Rt=zeros ( 2,2 );

	// ESTIMATORS
	vec mu0= "0.0 0.0 0.0 0.0";
	vec Qdiag ( "62 66 454 0.03" ); //zdenek: 0.01 0.01 0.0001 0.0001
	vec Rdiag ( "100 100" ); //var(diff(xth)) = "0.034 0.034"
	mat Q =diag( Qdiag );
	mat R =diag ( Rdiag );
	EKFfull Efix ( rx,ry,ru );
	Efix.set_est ( mu0, 1*eye ( 4 )  );
	Efix.set_parameters ( &fxu,&hxu,Q,R);

	RV rQR( "{Q R }", "4 2 ");
	EKFful_unQR EKU (rx,ry,ru,rQR);
	EKU.set_est ( mu0,  1*ones ( 4 ) );
	EKU.set_parameters ( &fxu,&hxu,diag(Qdiag),diag(Rdiag) );

	//QU model
	egamma Gcom(rQR);Gcom.set_parameters(ones(6),vec("1 1 1e4 1e10 1 1"));
/*	cout << Gcom.mean() <<endl;
	cout << Gcom.sample() <<endl;*/
	euni Ucom(rQR); Ucom.set_parameters(zeros(6),vec("60 60 453 0.03 100 100"));
/*	cout << Ucom.mean() <<endl;
	cout << Ucom.sample() <<endl;*/
	Array<epdf*> Coms(2);
	Coms(0) = &Gcom;
	Coms(1) = &Ucom;
	emix Eevol(rQR); 	Eevol.set_parameters("0.1 0.9", Coms);
// 	cout << Eevol.sample() <<endl;

	mepdf evolQR(Eevol);
	MPF<EKFful_unQR> M ( rx,rQR, evolQR, evolQR, Npar, EKU );
	M.set_est ( evolQR._epdf() );

	const epdf& Efix_ep = Efix._epdf();
	const epdf& M_ep = M._epdf();
	
	//LOG
	RV rUD( "{u_isa u_isb i_isa i_isb }");
	int X_log = L.add(rx,"X");
	int Efix_log = L.add(rx,"XF");
	RV tmp=concat(rQR,rx);
	int M_log = L.add(tmp,"M");
	L.init();

double dum=0.0;
vec dumvec = vec_1(1.0);
vec z=  evolQR.samplecond(dumvec,dum) ;
cout << z << endl;

	for ( int tK=1;tK<Ndat;tK++ ) {
		//Number of steps of a simulator for one step of Kalman
		for ( int ii=0; ii<Nsimstep;ii++ ) {
			sim_profile_steps1 ( Ww, true );
			pmsmsim_step ( Ww );
		};
		// simulation via deterministic model
		ut ( 0 ) = KalmanObs[0];
		ut ( 1 ) = KalmanObs[1];
		dt ( 0 ) = KalmanObs[2];
		dt ( 1 ) = KalmanObs[3];

		//ESTIMATE
		Efix.bayes(concat(dt,ut));
		//
		M.bayes(concat(dt,ut));
		
		//LOG
		L.logit(X_log, 	vec(x,4)); //vec from C-array
		L.logit(Efix_log, Efix_ep.mean() ); 
		L.logit(M_log, 	M_ep.mean() ); 
		
		L.step();
	}

	L.finalize();
	//L.itsave("sim_var.it");	
	

	return 0;
}