/*!
  \file
  \brief Simulation of disturbances in PMSM model, PWM and integration disturbances are distinguished
  \author Vaclav Smidl.

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

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

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

#include <stat/loggers.h>

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

using namespace itpp;
int main() {
	// Kalman filter
	int Ndat = 9000;
	double h = 1e-6;
	int Nsimstep = 125;
	
	dirfilelog L("exp/sim",Ndat);
	
	// internal model
	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 );
	// observation model
	OMpmsm hxu;

	vec mu0= "0.0 0.0 0.0 0.0";
//	vec Qdiag ( "0.01 0.01 0.01 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
	vec Qdiag ( "0.07 0.056 0.0007 0.0007" ); //zdenek: 0.01 0.01 0.0001 0.0001
	vec Rdiag ( "0.005 0.005" ); //var(diff(xth)) = "0.034 0.034"
	EKFfull KFE ( rx,ry,ru );
	KFE.set_est ( mu0, diag ( vec("1 1 1 3.1415") )  );
	KFE.set_parameters ( &fxu,&hxu,diag(Qdiag),diag(Rdiag));
	
	
	int X_log = L.add(rx,"X");
	int Xp_log = L.add(rx,"Xp");
	int Xp2_log = L.add(rx,"Xp2");
	int E_log = L.add(rx,"E");
	int V_log = L.add(rx,"V");
	int U_log = L.add(ru,"U");
	int U2_log = L.add(ru,"U2");
	int R_log = L.add(RV("{_ }","16"),"R");
	int Ww_log = L.add(RV("{_ }","1"),"W");
	int R2_log = L.add(RV("{_ }","16"),"R2");
//	int O_log = L.add(RV("{_ }","16"),"O");
	L.init();

	// SET SIMULATOR
	//pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
	pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 6e-6, h );
	double Ww=0.0;
	vec dt ( 2 );

	vec xm = zeros(4);
	vec xt = zeros(4);
	vec xp = zeros(4);
	vec xp2 = zeros(4);
	vec xp3 = zeros(4);
	vec u=zeros(2);
	vec u2=zeros(2);
	ldmat R0(eye(4),1e-6*ones(4));
	ldmat R(R0);
	ldmat R2(R0);
	
	double frg=0.9;
//	vec vecW="0. 0. 0.2 0.4 0.4 0.2 0.0 -0.4 -0.6 -0.6 -0.4 0.0 0.0";
	vec vecW="1 2 4 9 4 2 0 -4 -9 -16 -4 0 0 0";
	vecW*=10.0;

	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++ ) {
			//simulator
			//sim_profile_steps1(Ww);
			sim_profile_vec01t(Ww,vecW);
			pmsmsim_step ( Ww );
		};
		
		u(0)= KalmanObs[0];
		u(1)= KalmanObs[1];
		dt(0)= KalmanObs[2];
		dt(1)= KalmanObs[3];
		u2(0) = KalmanObs[4];
		u2(1) = KalmanObs[5];
		// Try what our model would predict!
		xp=fxu.eval(xm,u); 
		xp2=fxu.eval(xm,zeros(2)); //ZERO input!!!!!!!!
//		xp3=fxu.eval(xm,u2); 

//		KFE.bayes(concat(dt,u));
		// This is simulator prediction
		xt=vec(x,4); //vec from C-array
		//Covariance  
		R*=frg;
		R.add(R0,1-frg);
		R.opupdt(xt-xp,1.0);
		R2*=frg;
		R2.add(R0,1-frg);
		R2.opupdt(xt-xp2,1.0);
		xm = xt;
		L.logit(X_log, xt	); 
		L.logit(Xp_log, xp	); 
		L.logit(Xp2_log, xp2	); 
		L.logit(U_log, u	); 
		L.logit(U2_log, u2	); 
		L.logit(Ww_log, vec_1(Ww)); 
		L.logit(R_log, vec(R.to_mat()._data(), 16 )); //3.33=1/(1-0.7)
		L.logit(R2_log, vec(R2.to_mat()._data(), 16 )); 
//		L.logit(E_log, KFE._e()->mean()	); 
//		L.logit(O_log, vec(iCh._data(),16)); //3.33=1/(1-0.7)
//		L.logit(Efix_log, KFEep.mean() ); 
//		L.logit(M_log, 	Mep.mean() ); 
		
		L.step();
	}

	L.finalize(); 
	L.itsave("sim.it");
	return 0;
}