/*!
  \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 "pmsm.h"
#include "simulator.h"
#include <uibuilder.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;
	//! Setpoints of omega in timespans given by dt_prof
	vec profileWw;
	//! time-step for profiles
	double dt_prof;
	//! Number of miliseconds per discrete time step
	int Dt;
public:
	pmsmDS ( int Dt0 ) : Dt ( Dt0 ) {}
	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 );
	}
	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
		if ( t>=dt_prof*ind ) {
			if ( ind<profileWw.length() ) {
				ind++;
				//linear increase
				dW = profileWw ( ind )-profileWw ( ind-1 );
			}
			else {
				dW = 0;
			}
		}
		Ww += dW;
		//Simulate Dt seconds!
		for ( int i=0;i<Dt;i++ ) {	pmsmsim_step ( Ww );}
	};

	void log_add ( logger &L ) {
		L_x = L.add ( rx, "x" );
		L_oy = L.add ( ry, "oi" );
		L_ou = L.add ( ru, "ou" );
		L_iu = L.add ( ru, "iu" );
	}

	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] ) );
	}

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

class UIpmsm: public UIbuilder {
public:
	UIpmsm() :UIbuilder ( "pmsm" ) {}
	//Non-standard BUILD!! Does not create
	void build ( Setting &S, void** result ) const {

	};
};

class UIpmsmDS: public UIbuilder {
	static void tmp_set ( Setting &S, pmsmDS* tmp ) {
		tmp->set_parameters ( S["Rs"], S["Ls"], S["Fmag"], S["Bf"], S["p"], S["kp"], \
		                      S["J"], S["Uc"], S["DT"], 1.0e-6 );
	};
public:
	UIpmsmDS() :UIbuilder ( "pmsmDS" ) {};
	bdmroot* build ( Setting &S ) const {
		pmsmDS* tmp = new pmsmDS ( S["DT"] );
		//allowing recursive Settings
		UIcall<pmsmDS*> ( S["params"], &tmp_set , tmp );
	};

};
UIREGISTER(UIpmsmDS);