#ifndef PMSM_H
#define PMSM_H

#include <stat/libFN.h>

/*! \defgroup PMSM 
@{
*/

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

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

//! 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;

public:
	IMpmsm() :diffbifn (rx.count(), rx, ru ) {};
	//! 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 + 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 ( 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 ) {
		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 ) = 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" );};

};

//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsm2o : public diffbifn {
	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() :diffbifn (rx.count(), rx, ru ) {};
	//! 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*d2th; // <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;
			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;
		}

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

};

//! 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;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
		//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" );};

};

//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
class OMpmsm: public diffbifn {
public:
	OMpmsm() :diffbifn (2, rx,ru ) {};

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

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