/*!
  \file
  \brief DataSource for experiments with realistic simulator of the PMSM model
  \author Vaclav Smidl.

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

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

#include <stat/loggers.h>
#include <estim/libKF.h>
#include "simulator.h"
#include "pmsm.h"

//! Simulator of PMSM machine with predefined profile on omega
class pmsmDS : public DS {

protected:
	//! indeces of logged variables
	int L_x, L_ou, L_oy, L_iu, L_optu;
	//! Setpoints of omega in timespans given by dt_prof
	vec profileWw;
	//! Setpoints of Mz in timespans given by dt_prof
	vec profileMz;
	//! time-step for profiles
	double dt_prof;
	//! Number of miliseconds per discrete time step
	int Dt;
	//! options for logging, - log predictions of 'true' voltage
	bool opt_modu;
	//! options for logging, - 
public:
	//! Constructor with fixed sampling period
	pmsmDS ()   {Dt=125; Drv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub }" );}
	void set_parameters ( double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0 ) {
		pmsmsim_set_parameters ( Rs0, Ls0, Fmag0, Bf0, p0, kp0, J0, Uc0, DT0, dt0 );
	}
	//! parse options: "modelu" => opt_modu=true;
	void set_options ( string &opt ) {
		opt_modu = ( opt.find ( "modelu" ) ==string::npos );
	}
	void getdata ( vec &dt ) {dt=vec ( KalmanObs,6 );}
	void write ( vec &ut ) {}

	void step() {
		static int ind=0;
		static double dW; // increase of W
		static double Ww; // W
		static double Mz; // W
		if ( t>=dt_prof*ind ) {
			ind++;
			// check omega profile and set dW
			if ( ind<profileWw.length() ) {
				//linear increase
				if ( profileWw.length() ==1 ) {
					Ww=profileWw ( 0 ); dW=0.0;
				}
				else {
					dW = profileWw ( ind )-profileWw ( ind-1 );
					dW *=125e-6/dt_prof;
				}
			}
			else {
				dW = 0;
			}
			// Check Mz profile and set Mz
			if ( ind<profileMz.length() ) {
				//sudden increase
				Mz = profileMz(ind);
			}
			else {
				Mz = 0;
			}
		}
		Ww += dW;
		//Simulate Dt seconds!
		for ( int i=0;i<Dt;i++ ) {	pmsmsim_step ( Ww , Mz);}
	};

	void log_add ( logger &L ) {
		L_x = L.add ( rx, "x" );
		L_oy = L.add ( ry, "o" );
		L_ou = L.add ( ru, "o" );
		L_iu = L.add ( ru, "t" );
		// log differences
		if ( opt_modu ) {
			L_optu = L.add ( ru, "model" );
		}
	}

	void logit ( logger &L ) {
		L.logit ( L_x, vec ( x,4 )	);
		L.logit ( L_oy, vec_2 ( KalmanObs[2],KalmanObs[3] ) );
		L.logit ( L_ou, vec_2 ( KalmanObs[0],KalmanObs[1] ) );
		L.logit ( L_iu, vec_2 ( KalmanObs[4],KalmanObs[5] ) );
		if ( opt_modu ) {
			double sq3=sqrt ( 3.0 );
			double ua,ub;
			double i1=x[0];
			double i2=0.5* ( -i1+sq3*x[1] );
			double i3=0.5* ( -i1-sq3*x[1] );
			double u1=KalmanObs[0];
			double u2=0.5* ( -u1+sq3*KalmanObs[1] );
			double u3=0.5* ( -u1-sq3*KalmanObs[1] );

			double du1=0.7* ( double ( i1>0.1 ) - double ( i1<-0.1 ) ) +0.05*i1;
			double du2=0.7* ( double ( i2>0.1 ) - double ( i2<-0.1 ) ) +0.05*i2;
			double du3=0.7* ( double ( i3>0.1 ) - double ( i3<-0.1 ) ) +0.05*i3;
			ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
			ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
			L.logit ( L_optu , vec_2 ( ua,ub ) );
		}
		
	}

	void set_profile ( double dt, const vec &Ww, const vec &Mz ) {dt_prof=dt; profileWw=Ww; profileMz=Mz;}
};

//! This class behaves like BM but it is evaluating EKF
class pmsmCRB : public EKFCh{
	protected:
		vec interr;
		vec old_true;
		vec secder;
		int L_CRB;
		int L_err;
		int L_sec;
	public:
	//! constructor
	pmsmCRB():EKFCh(){old_true=zeros(6);}

	void bayes(const vec &dt){
		//assume we know state exactly:
		vec true_state=vec(x,4); // read from pmsm
		est.set_mu(true_state);
		
		//integration error
		old_true(4)=KalmanObs[4];
		old_true(5)=KalmanObs[5];// add U
		interr = (true_state - pfxu->eval(old_true));
		
		//second derivative
		IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu);
		if (pf) {secder=pf->eval2o(vec_2(KalmanObs[4],KalmanObs[5]));}
				
		EKFCh::bayes(dt);
		old_true.set_subvector(0,true_state);
	}
	
	void log_add(logger &L, const string &name="" ){
		L_CRB=L.add(rx,"crb");
		L_err=L.add(rx,"err");
		L_sec=L.add(rx,"d2");
	}
	void logit(logger &L){
		L.logit(L_err, interr);
		L.logit(L_CRB,diag(est._R().to_mat()));
		L.logit(L_sec,secder);
	}
};