/*!
  \file
  \brief Simulation of disturbances in PMSM model, EKF runs with simulated covariances
  \author Vaclav Smidl.

  \ingroup PMSM
  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

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

#include <itpp/itbase.h>
#include <stat/libFN.h>
#include <estim/libKF.h>
//#include <estim/libPF.h>
#include <math/chmat.h>

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

#include <stat/loggers.h>

using namespace bdm;

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

	dirfilelog L("exp/sim_var2",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 utm ( 2 );
	vec dut ( 2 );
	vec dit (2);
	vec x2o(2);
	vec xtm=zeros ( 4 );
	vec xte=zeros ( 4 );
	vec xdif=zeros ( 4 );
	vec xt ( 4 );
	vec ddif=zeros(2);
	IMpmsm2o 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);

	EKFfull Eop ( rx,ry,ru );
	Eop.set_est ( mu0, 1*eye ( 4 ) );
	Eop.set_parameters ( &fxu,&hxu,Q,R);

	EKFfull Edi ( rx,ry,ru );
	Edi.set_est ( mu0, 1*eye ( 4 ) );
	Edi.set_parameters ( &fxu,&hxu,Q,R);

	const epdf& Efix_ep = Efix._epdf();
	const epdf& Eop_ep = Eop._epdf();
	const epdf& Edi_ep = Edi._epdf();
	
	//LOG
	RV rQ( "{Q }", "16");
	RV rR( "{R }", "4");
	RV rUD( "{u_isa u_isb i_isa i_isb }", ones_i(4));
	RV rDu("{dux duy }",ones_i(2));
	RV rDi("{disa disb }",ones_i(2));
	int X_log = L.add(rx,"X");
	int X2o_log = L.add(rx,"X2o");
	int Xdf_log = L.add(rx,"Xdf");
	int Efix_log = L.add(rx,"XF");
	int Eop_log = L.add(rx,"XO");
	int Edi_log = L.add(rx,"XD");
	int Q_log = L.add(rQ,"Q");
	int R_log = L.add(rR,"R");
	int D_log = L.add(rUD,"D");
	int Du_log = L.add(rDu,"Du");
	int Di_log = L.add(rDi,"Di");
	L.init();

	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 , false);
			pmsmsim_step ( Ww );
		};
		// simulation via deterministic model
		ut ( 0 ) = KalmanObs[4];
		ut ( 1 ) = KalmanObs[5];
		
		x2o = fxu.eval2o(utm-ut);
		utm = ut;
		
		dt ( 0 ) = KalmanObs[2];
		dt ( 1 ) = KalmanObs[3];
		dut ( 0 ) = KalmanObs[4];
		dut ( 1 ) = KalmanObs[5];
		dit ( 0 ) = KalmanObs[8];
		dit ( 1 ) = KalmanObs[9];

		xte = fxu.eval ( xtm,ut );
		//Results:
		// in xt we have simulation according to the model
		// in x we have "reality"
		xt ( 0 ) =x[0];xt ( 1 ) =x[1];xt ( 2 ) =x[2];xt ( 3 ) =x[3];
		xdif = xt-xte; //xtm is a copy of x[]
		if (xdif(0)>3.0){
			cout << "here" <<endl;
			}
		if ( xdif ( 3 ) >pi ) xdif ( 3 )-=2*pi;
		if ( xdif ( 3 ) <-pi ) xdif ( 3 ) +=2*pi;
		
		ddif = hxu.eval(xt,ut) - dit;

		//Rt = 0.9*Rt + xdif^2
		Qt*=0.1;
		Qt += 10*outer_product ( xdif,xdif ); //(x-xt)^2

		if (Qt(0,0)>3.0){
			cout << "here" <<endl;
			}
		// For future ref.
		xtm = xt;

		Rt*=0.1;
//		Rt += 10*outer_product ( ddif,ddif ); //(x-xt)^2

		//ESTIMATE
		Efix.bayes(concat(dt,ut));
		//
		Eop.set_parameters ( &fxu,&hxu,(Qt+1e-8*eye(4)),(Rt+1e-6*eye(2)));
		Eop.bayes(concat(dt,ut));
		//
		Edi.set_parameters ( &fxu,&hxu,(diag(diag(Qt))+1e-16*eye(4)), (diag(diag(Rt))+1e-3*eye(2)));
		Edi.bayes(concat(dt,ut));
		
		//LOG
		L.logit(X_log, 	vec(x,4)); //vec from C-array
		L.logit(X2o_log, x2o); 
		L.logit(Xdf_log, xdif); 
		L.logit(Efix_log, Efix_ep.mean() ); 
		L.logit(Eop_log, 	Eop_ep.mean() ); 
		L.logit(Edi_log, 	Edi_ep.mean() ); 
		L.logit(Q_log, 	vec(Qt._data(),16) ); 
		L.logit(R_log,		vec(Rt._data(),4) ); 
		L.logit(D_log,	vec(KalmanObs,4) ); 
		L.logit(Du_log,	dut ); 
		L.logit(Di_log,	dit ); 
		
		L.step();
	}

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

	return 0;
}