#ifndef PMSM_H
#define PMSM_H

#include <math/functions.h>
#include "base/user_info.h"
#include "ekf_example/reference_Q15.h"

/*! \defgroup PMSM 
@{
*/

using namespace bdm;

//TODO hardcoded RVs!!!
// RV rx ( "{ia ib om th }");
// RV ru ( "{o_ua o_ub }");
// RV ry ( "{oia oib }");

// class uipmsm : public uibase{
// 	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
// };

inline double va_drop_interp(double i){
	//double va_char[16]={0,10,50,100,200,300,500,1000, 0,1,1.8,2.4,3.2,3.8,4.8,6.8};
	//*
	double si = sign(i);
	i = abs(i);
	double du;
		
	if(i < 0.1) du = 5.0*i;
     else if(i < 0.5) du = 1.0*(i-0.1) + 0.5;
     else if(i < 1.0) du = 0.6*(i-0.5) + 0.9;
     else if(i < 2.0) du = 0.4*(i-1.0) + 1.2;
     else if(i < 3.0) du = 0.3*(i-2.0) + 1.6;
     else if(i < 5.0) du = 0.25*(i-3.0) + 1.9;				
     else du = 0.2*(i - 5.0) + 2.4;
    	
	return si*du*1.0;
	
	/*/
	double msi = sign(i);
	
	i = abs(i) - 0.3;
	
	double si = sign(i);
	i = abs(i);
	double du;
	
	if(i < 0.0654) du = 5.0*i;
	else if(i < 0.3225) du = 1.0*(i-0.0654) + 0.3225;
	else if(i < 0.654) du = 0.6*(i-0.3225) + 0.5805;
	else if(i < 1.29) du = 0.4*(i-0.654) + 0.774;
	else if(i < 1.935) du = 0.3*(i-1.29) + 1.032;
	else if(i < 3.225) du = 0.25*(i-1.935) + 1.2255;				
	else du = 0.2*(i - 3.225) + 1.548;
		
	return msi*(si*du+0.558);
	/*/
}

//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsm : public diffbifn {
protected:
	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;

	bool compensate;
	bool cutoff;
public:
	IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=false;cutoff=true;};
	//! Set mechanical and electrical variables
	virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}

	void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
/*		ua=u0[0];
		ub=u0[1];
		return;*/
		double sq3=sqrt ( 3.0 );
		double i1=x0(0);
		double i2=0.5* ( -i1+sq3*x0[1] );
		double i3=0.5* ( -i1-sq3*x0[1] );
		double u1=u0(0);
		double u2=0.5* ( -u1+sq3*u0(1) );
		double u3=0.5* ( -u1-sq3*u0(1) );
		//
		double du1=va_drop_interp(i1);
		double du2=va_drop_interp(i2);
		double du3=va_drop_interp(i3);
		/*/
		double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
		double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
		double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
		*/
		ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
		ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
	}

	vec eval ( const vec &x0, const vec &u0 ) {
		// last state
		const double &iam = x0 ( 0 );
		const double &ibm = x0 ( 1 );
		const double &omm = x0 ( 2 );
		const double &thm = x0 ( 3 );
		double uam;
		double ubm;

		if (compensate){
			modelpwm(x0,u0,uam,ubm);
		} else {
			uam = u0(0);
			ubm = u0(1);
		}
		
		
		vec xk( 4 );
		//ia
		xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
		//ib
		xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
		//om
		xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
		//th
		xk ( 3 ) = thm + omm*dt; // <0..2pi>
		if (cutoff) {
			if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
			if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
		}
		return xk;
	}

	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		const double &iam = x0 ( 0 );
		const double &ibm = x0 ( 1 );
		const double &omm = x0 ( 2 );
		const double &thm = x0 ( 3 );
		// d ia
		A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
		A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
		// d ib
		A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
		A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
		// d om
		A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
		A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
		A ( 2,2 ) = 1.0;
		A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
		// d th
		A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
	}

	void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};

	void from_setting( const Setting &root )
	{	

		const SettingResolver& params_b=root["params"];
		const Setting& params=params_b.result;
		
		set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
                         params["kp"], params["p"], params["J"], 0.0);
		int comp=0;
		if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
		int cuto=0;
		if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
	};

	// TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( IMpmsm );

//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsmOT : public diffbifn {
protected:
	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
	
	bool compensate;
	bool cutoff;
public:
	IMpmsmOT() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;compensate=false;cutoff=true;};
	//! Set mechanical and electrical variables
	virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
	
	void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
		/*		ua=u0[0];
		 *		ub=u0[1];
		 *		return;*/
		double sq3=sqrt ( 3.0 );
		double i1=x0(0);
		double i2=0.5* ( -i1+sq3*x0[1] );
		double i3=0.5* ( -i1-sq3*x0[1] );
		double u1=u0(0);
		double u2=0.5* ( -u1+sq3*u0(1) );
		double u3=0.5* ( -u1-sq3*u0(1) );
		
		double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
		double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
		double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
		ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
		ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
	}
	
	vec eval ( const vec &x0, const vec &u0 ) {
		// last state
		const double &omm = x0 ( 0 );
		const double &thm = x0 ( 1 );
		double uam;
		double ubm;
		
		if (compensate){
			modelpwm(x0,u0,uam,ubm);
		} else {
			uam = u0(0);
			ubm = u0(1);
		}
		
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		vec xk( 2 );
		//ia
		//om
		xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
		//th
		xk ( 1 ) = thm + omm*dt; // <0..2pi>
		if (cutoff) {
			if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
			if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
		}
		return xk;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
//		const double &omm = x0 ( 0 );
		const double &thm = x0 ( 1 );

		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		// d ia
		// d om
		A ( 0,0 ) = 1.0;
		A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
		// d th
		A ( 1,0 ) = dt; 
		A ( 1,1 ) = 1.0;
	}
	
	void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
	
	void from_setting( const Setting &root )
	{	
		
		const SettingResolver& params_b=root["params"];
		const Setting& params=params_b.result;
		
		set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
		params["kp"], params["p"], params["J"], 0.0);
		int comp=0;
		if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
		int cuto=0;
		if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
	};
	
	// TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( IMpmsmOT );

//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsmOTM : public diffbifn {
protected:
	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
	
	bool compensate;
	bool cutoff;
public:
	IMpmsmOTM() :diffbifn ( ) {dimy=2; dimx = 3; dimu=4; dimc=6;compensate=false;cutoff=true;};
	//! Set mechanical and electrical variables
	virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
	
	void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
		/*		ua=u0[0];
		 *		ub=u0[1];
		 *		return;*/
		double sq3=sqrt ( 3.0 );
		double i1=x0(0);
		double i2=0.5* ( -i1+sq3*x0[1] );
		double i3=0.5* ( -i1-sq3*x0[1] );
		double u1=u0(0);
		double u2=0.5* ( -u1+sq3*u0(1) );
		double u3=0.5* ( -u1-sq3*u0(1) );
		
		double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
		double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
		double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
		ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
		ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
	}
	
	vec eval ( const vec &x0, const vec &u0 ) {
		// last state
		const double &omm = x0 ( 0 );
		const double &thm = x0 ( 1 );
		const double &Mm  = x0 ( 2 );
		double uam;
		double ubm;
		
		if (compensate){
			modelpwm(x0,u0,uam,ubm);
		} else {
			uam = u0(0);
			ubm = u0(1);
		}
		
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		vec xk( 3 );
		//ia
		//om
		xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mm;
		//th
		xk ( 1 ) = thm + omm*dt; // <0..2pi>
		xk (2) = Mm;
		
		if (cutoff) {
			if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
			if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
		}
		return xk;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
//		const double &omm = x0 ( 0 );
		const double &thm = x0 ( 1 );
//		const double &Mm  = x0 ( 2 );
		
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		// d ia
		// d om
		A ( 0,0 ) = 1.0;
		A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
		A ( 0,2 ) = -p/J*dt;
		// d th
		A ( 1,0 ) = dt; 
		A ( 1,1 ) = 1.0;
		A ( 1,2 ) = 0.0;
		// d M
		A ( 2,0 ) = 0.0;
		A ( 2,1 ) = 0.0;
		A ( 2,2 ) = 1.0;
	}
	
	void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
	
	void from_setting( const Setting &root )
	{	
		
		const SettingResolver& params_b=root["params"];
		const Setting& params=params_b.result;
		
		set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
		params["kp"], params["p"], params["J"], 0.0);
		int comp=0;
		if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
		int cuto=0;
		if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
	};
	
	// TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( IMpmsmOTM );

//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$ -- Scaled to [-1,1]
class IMpmsmOTsc : public diffbifn {
protected:
	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
	
	bool cutoff;
public:
	IMpmsmOTsc() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;cutoff=true;};
	//! Set mechanical and electrical variables
	virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
	
	
	vec eval ( const vec &x0, const vec &u0 ) {
		// last state
		const double &omm = x0 ( 0 );
		const double &thm = x0 ( 1 );
		double uam;
		double ubm;
		
		uam = u0(0);
		ubm = u0(1);
		
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		vec xk( 2 );
		//ia
		//om
		xk ( 0 ) = omm + kp*p*p * Ypm/J*dt *Iref/Wref* ( ibm * cos ( thm*Thetaref/(1<<15) )-iam * sin ( thm*Thetaref/(1<<15) ) );
		//th
		xk ( 1 ) = thm + omm*Wref/Thetaref*dt; // <0..2pi>
		if (cutoff) {
			if ( xk ( 1 ) >(1<<15) ) xk ( 1 )=-(1<<15);
			if ( xk ( 1 ) <-(1<<15) ) xk ( 1 ) =(1<<15);
		}
		return xk;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		const double &omm = x0 ( 0 );
		const double &thm = x0 ( 1 );
		
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		// d ia
		// d om
		A ( 0,0 ) = 1.0;
		A ( 0,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
		// d th
		A ( 1,0 ) = dt*Wref/Thetaref; 
		A ( 1,1 ) = 1.0;
	}
	
	void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
	
	void from_setting( const Setting &root )
	{	
		
		const SettingResolver& params_b=root["params"];
		const Setting& params=params_b.result;
		
		set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
		params["kp"], params["p"], params["J"], 0.0);
		int cuto=0;
		if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
	};
	
	// TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( IMpmsmOTsc );


//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsmDQ : public diffbifn {
protected:
	double Rs, Ld, Lq, dt, Ypm, kp, p,  J, Mz;
	
	bool compensate;
	bool cutoff;
public:
	IMpmsmDQ() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
	//! Set mechanical and electrical variables
	virtual void set_parameters ( double Rs0, double Ld0, double Lq0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ld=Ld0; Lq=Lq0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
	
	void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
		/*		ua=u0[0];
		 *		ub=u0[1];
		 *		return;*/
		double sq3=sqrt ( 3.0 );
		double i1=x0(0);
		double i2=0.5* ( -i1+sq3*x0[1] );
		double i3=0.5* ( -i1-sq3*x0[1] );
		double u1=u0(0);
		double u2=0.5* ( -u1+sq3*u0(1) );
		double u3=0.5* ( -u1-sq3*u0(1) );
		
		double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
		double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
		double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
		ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
		ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
	}
	
	vec eval ( const vec &x0, const vec &u0 ) {
		// last state
		const double &idm = x0 ( 0 );
		const double &iqm = x0 ( 1 );
		const double &omm = x0 ( 2 );
		const double &thm = x0 ( 3 );
		double uam;
		double ubm;
		
		if (compensate){
			modelpwm(x0,u0,uam,ubm);
		} else {
			uam = u0(0);
			ubm = u0(1);
		}
		
		vec xk( 4 );
		//id
		xk ( 0 ) = ( 1.0- Rs/Ld*dt ) * idm + Lq/Ld*iqm*dt*omm + dt/Ld*(cos(thm)*uam+sin(thm)*ubm);
		//iq
		xk ( 1 ) = ( 1.0- Rs/Lq*dt ) * iqm - Ypm/Lq*dt*omm - Ld/Lq*dt*idm*omm + dt/Lq*(-sin(thm)*uam+cos(thm)*ubm);
		//om
		xk ( 2 ) = omm + kp*p*p /J*dt* ( (Ld-Lq)*idm+ Ypm)*iqm - p/J*dt*Mz;
		//th
		xk ( 3 ) = thm + omm*dt; // <0..2pi>
		if (cutoff) {
			if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
			if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
		}
		return xk;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		const double &idm = x0 ( 0 );
		const double &iqm = x0 ( 1 );
		const double &omm = x0 ( 2 );
		const double &thm = x0 ( 3 );
		
		double uam;
		double ubm;
		if (compensate){
			modelpwm(x0,u0,uam,ubm);
		} else {
			uam = u0(0);
			ubm = u0(1);
		}
		// d id
		A ( 0,0 ) = ( 1.0- Rs/Ld*dt ); A ( 0,1 ) = Lq/Ld*dt*omm;
		A ( 0,2 ) = Lq/Ld*iqm*dt; A ( 0,3 ) = dt/Ld*(-sin(thm)*uam+cos(thm)*ubm);
		// d iq
		A ( 1,0 ) = -Ld/Lq*omm*dt ; A ( 1,1 ) = ( 1.0- Rs/Lq*dt );
		A ( 1,2 ) = -Ypm/Lq*dt- Ld/Lq*dt*idm; A ( 1,3 ) = dt/Lq*(-cos(thm)*uam-sin(thm)*ubm);
		// d om
		A ( 2,0 ) = kp*p*p /J*dt* ( Ld-Lq)*iqm;
		A ( 2,1 ) = kp*p*p /J*dt* ((Ld-Lq)*idm+Ypm);
		A ( 2,2 ) = 1.0;
		A ( 2,3 ) = 0;
		// d th
		A ( 3,0 ) = 0.0; 
		A ( 3,1 ) = 0.0; 
		A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
	}
	
	void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
	
	void from_setting( const Setting &root )
	{	
		
		const SettingResolver& params_b=root["params"];
		const Setting& params=params_b.result;
		
		set_parameters ( params["Rs"], params["Ld"],params["Lq"], 125e-6, params["Fmag"], \
		params["kp"], params["p"], params["J"], 0.0);
		int comp=0;
		if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
		int cuto=0;
		if (UI::get(cuto,root,"cutoff",UI::optional)){
			cutoff=(cuto==1);
		}
	};
	
	// TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( IMpmsmDQ );


//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsm2o : public IMpmsm {
	protected:
//		double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
		//! store first derivatives for the use in second derivatives
		double dia, dib, dom, dth;
		//! d2t = dt^2/2, cth = cos(th), sth=sin(th)
		double d2t, cth, sth;
		double iam, ibm, omm, thm, uam, ubm;
	public:
		IMpmsm2o() :IMpmsm () {};
	//! Set mechanical and electrical variables
		void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0; d2t=dt*dt/2;}

		vec eval ( const vec &x0, const vec &u0 ) {
		// last state
			iam = x0 ( 0 );
			ibm = x0 ( 1 );
			omm = x0 ( 2 );
			thm = x0 ( 3 );
			uam = u0 ( 0 );
			ubm = u0 ( 1 );

			cth = cos(thm);
			sth = sin(thm);
			
			dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
			dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
			dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
			dth = omm;
						
			vec xk=zeros ( 4 );
			xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
			xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
			xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
			xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
			
			if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
			if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
			return xk;
		}

		//! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
		vec eval2o(const vec &du){
			double dua = du ( 0 )/dt;
			double dub = du ( 1 )/dt;
			
			vec xth2o(4);
			xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
			xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
			xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
			xth2o(3) = dom;
			// multiply by dt^2/2
			xth2o*=d2t/2;
			return xth2o;
		}
		void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
			 iam = x0 ( 0 );
			 ibm = x0 ( 1 );
			 omm = x0 ( 2 );
			 thm = x0 ( 3 );
		// d ia
			A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
			A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
		// d ib
			A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
			A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
		// d om
			A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
			A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
			A ( 2,2 ) = 1.0;
			A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
		// d th
			A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
			// FOR d2t*dom!!!!!!!!!
/*			A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
			A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
			A ( 3,2 ) = dt;
			A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
		}

		void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};

};


UIREGISTER ( IMpmsm2o );

//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
class IMpmsmStat : public IMpmsm {
	public:
	IMpmsmStat() :IMpmsm() {};
	//! Set mechanical and electrical variables
	void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}

	vec eval ( const vec &x0, const vec &u0 ) {
		// last state
		double iam = x0 ( 0 );
		double ibm = x0 ( 1 );
		double omm = x0 ( 2 );
		double thm = x0 ( 3 );
		double uam = u0 ( 0 );
		double ubm = u0 ( 1 );

		vec xk=zeros ( 4 );
		//ia
		xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
		//ib
		xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
		//om
		xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
		//th
		xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
		return xk;
	}

	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
//		double iam = x0 ( 0 );
//		double ibm = x0 ( 1 );
		double omm = x0 ( 2 );
		double thm = x0 ( 3 );
		// d ia
		A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
		A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
		// d ib
		A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
		A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
		// d om
		A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
		A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
		A ( 2,2 ) = 1.0;
		A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
		// d th
		A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
	}

	void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};

};

UIREGISTER ( IMpmsmStat );


//! State for PMSM with unknown Mz
class IMpmsmMz: public IMpmsm{
	public:
		IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
	//! extend eval by Mz
		vec eval ( const vec &x0, const vec &u0 ) {
			vec x(4);
			Mz = x0(4); //last of the state is Mz
		
		//teh first 4 states are same as before (given that Mz is set)
			x=IMpmsm::eval(x0,u0); // including model of drops!
			return concat(x,Mz);
		}
		void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		//call initial
			if (full) A.clear();
			IMpmsm::dfdx_cond(x0,u0,A,full);
			A(2,4)=- p/J*dt;
			A(4,4)=1.0;
		}	
};

UIREGISTER ( IMpmsmMz );

//! State for PMSM with unknown Mz
class IMpmsmStatMz: public IMpmsmStat{
	public:
		IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
	//! extend eval by Mz
		vec eval ( const vec &x0, const vec &u0 ) {
			vec x(4);
			Mz = x0(4); //last of the state is Mz
		
		//teh first 4 states are same as before (given that Mz is set)
			x=IMpmsmStat::eval(x0,u0); // including model of drops!
			return concat(x,Mz);
		}
		void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		//call initial
			if (full) A.clear();
			IMpmsmStat::dfdx_cond(x0,u0,A,full);
			A(2,4)=- p/J*dt;
			A(4,4)=1.0;
		}	
};

UIREGISTER ( IMpmsmStatMz );


//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
class OMpmsm: public diffbifn {
public:
	OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};

	vec eval ( const vec &x0, const vec &u0 ) {
		vec y ( 2 );
		y ( 0 ) = x0 ( 0 );
		y ( 1 ) = x0 ( 1 );
		return y;
	}

	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		A.clear();
		A ( 0,0 ) = 1.0;
		A ( 1,1 ) = 1.0;
	}
};

UIREGISTER ( OMpmsm );

//! Observation model for PMSM drive with roundoff-errors
class OMpmsmRO: public diffbifn {
	public:
		OMpmsmRO() :diffbifn () {dimy=2;dimx=4;dimu=0;};
		
		vec eval ( const vec &x0, const vec &u0 ) {
			vec y ( 2 );
/*			y ( 0 ) = x0 ( 0 );
			y ( 1 ) = x0 ( 1 );*/
			
			double istep=0.085;
			y[ 0 ] = istep*itpp::round( x0[ 0 ]/ istep) ;
			y[ 1 ] = istep*itpp::round( x0[ 1 ]/ istep);
			
			return y;
		}
		
		void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
			A.clear();
			A ( 0,0 ) = 1.0;
			A ( 1,1 ) = 1.0;
		}
};

UIREGISTER ( OMpmsmRO );

class OMpmsmROMz: public OMpmsmRO{
public:
	OMpmsmROMz() :OMpmsmRO() {dimy=2;dimx=5;dimu=0;};
};
UIREGISTER ( OMpmsmROMz );

//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
class OMpmsm4: public diffbifn {
	public:
		OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};

		vec eval ( const vec &x0, const vec &u0 ) {
			vec y ( 4 );
			y  = x0 ;
			return y;
		}

		void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
			A.clear();
			A(0,0)=1.0;
			A(1,1)=1.0;
		}
};

UIREGISTER ( OMpmsm4 );

//! Observation model for PMSM drive id d-q coordinates
class OMpmsmDQ: public diffbifn {
public:
	OMpmsmDQ() :diffbifn () {dimy=2;dimx=4;dimu=2;};
	
	vec eval ( const vec &x0, const vec &u0 ) {
		vec y ( 2 );
		double ct = cos(x0(3));
		double st = sin(x0(3));
		y(0)  = ct*x0(0)-st*x0(1);
		y(1)  = st*x0(0)+ct*x0(1);
		
		return y;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		double ct = cos(x0(3));
		double st = sin(x0(3));
		A(0,0) = ct;
		A(1,0) = -st;
		A(0,1) = st;
		A(1,1) = ct;
		
		A(0,2) = 0.0; 
		A(1,2) = 0.0;
		A(0,3) = -st*x0(0)-ct*x0(1);
		A(1,3) = ct*x0(0)-st*x0(1);
	}
};

UIREGISTER ( OMpmsmDQ );

//! Observation model for PMSM drive in reduced form coordinates
class OMpmsmOT: public diffbifn {
public:
	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
	
	virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
	
	OMpmsmOT() :diffbifn () {dimy=2;dimx=2;dimu=4;};
	
	vec eval ( const vec &x0, const vec &u0 ) {
		vec y ( 2 );
		const double &omm = x0(0);
		const double &thm = x0(1);
		
		const double &uam = u0 ( 0 );
		const double &ubm = u0 ( 1 );
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
		//ib
		y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
		
		return y;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		const double &omm = x0(0);
		const double &thm = x0(1);
				
		A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
		A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
		// d ib
		A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
		A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
		
		
	}

void from_setting( const Setting &root )
{	
	
	const SettingResolver& params_b=root["params"];
	const Setting& params=params_b.result;
	
	set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
	params["kp"], params["p"], params["J"], 0.0);
};

};

UIREGISTER ( OMpmsmOT );

//! Observation model for PMSM drive in reduced form coordinates
class OMpmsmOTM: public diffbifn {
public:
	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
	
	virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
	
	OMpmsmOTM() :diffbifn () {dimy=2;dimx=2;dimu=4;};
	
	vec eval ( const vec &x0, const vec &u0 ) {
		vec y ( 2 );
		const double &omm = x0(0);
		const double &thm = x0(1);
		
		const double &uam = u0 ( 0 );
		const double &ubm = u0 ( 1 );
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
		//ib
		y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
		
		return y;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		const double &omm = x0(0);
		const double &thm = x0(1);
		
		A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
		A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
		A( 0,2) = 0.0;
		// d ib
		A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
		A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
		A(1,2) = 0.0;
		
	}
	
	void from_setting( const Setting &root )
	{	
		
		const SettingResolver& params_b=root["params"];
		const Setting& params=params_b.result;
		
		set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
		params["kp"], params["p"], params["J"], 0.0);
	};
	
};

UIREGISTER ( OMpmsmOTM );



//! Observation model for PMSM drive in reduced form coordinates -- scaled to [-1,1]
class OMpmsmOTsc: public diffbifn {
public:
	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
	
	virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
	
	OMpmsmOTsc() :diffbifn () {dimy=2;dimx=2;dimu=4;};
	
	vec eval ( const vec &x0, const vec &u0 ) {
		vec y ( 2 );
		const double &omm = x0(0);
		const double &thm = x0(1);
		
		const double &uam = u0 ( 0 );
		const double &ubm = u0 ( 1 );
		const double &iam = u0 ( 2 );
		const double &ibm = u0 ( 3 );
		
		y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm *Wref/Iref* sin ( thm*Thetaref/(1<<15) ) + (1<<15)*uam*dt/Ls /Iref;
		//ib
		y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm *Wref/Iref* cos ( thm*Thetaref/(1<<15) ) + (1<<15)*ubm*dt/Ls /Iref;
		
		return y;
	}
	
	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
		const double &omm = x0(0);
		const double &thm = x0(1);
		
		A ( 0,0 ) = Ypm/Ls*dt* Wref/Iref*sin ( thm*Thetaref/(1<<15) ); 
		A ( 0,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( cos ( thm*Thetaref/(1<<15) ) );
		// d ib
		A ( 1,0 ) = -Ypm/Ls*dt* Wref/Iref*cos ( thm*Thetaref/(1<<15) ); 
		A ( 1,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( sin ( thm*Thetaref/(1<<15) ) );
		
		
	}
	
	void from_setting( const Setting &root )
	{	
		
		const SettingResolver& params_b=root["params"];
		const Setting& params=params_b.result;
		
		set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
		params["kp"], params["p"], params["J"], 0.0);
	};
	
};

UIREGISTER ( OMpmsmOTsc );


/*!@}*/
#endif //PMSM_H