#ifndef PMSMFIL_H
#define PMSMFIL_H

#include "pmsm.h"
#include <estim/particles.h>

/*! \defgroup PMSM
@{
*/

using namespace bdm;

//* FAILURE :(((
class MPFpmsm: public MPF {
private:

	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;


//! modelling theta as normal random walk with 2pi correction
	class rwtheta: public mpdf_internal<enorm<fsqmat> > {
		double om_hat_dt;
		double om_var;
		double rth;
		MPFpmsm& p;
	public:
		rwtheta ( MPFpmsm& p0 ) :p ( p0 ) {};
		void set_parameters ( double omh0, double omv0, double rth0 ) {
			om_hat_dt=omh0*p.dt;
			om_var=omv0;
			rth=rth0;

			mat R ( 1,1 );
			R ( 0,0 ) =p.dt*p.dt*om_var+rth;
			vec mu ( 1 );
			mu ( 0 ) = om_hat_dt;
			iepdf.set_parameters ( mu, fsqmat ( R ) );
		}
		vec samplecond ( const vec &cond ) {
			iepdf._mu() =om_hat_dt+cond ( 0 );

			//
//			iepdf._mu()= x[3];// ----------
			vec th=iepdf.sample();
			if ( th ( 0 ) >pi ) th ( 0 )-=2*pi;
			if ( th ( 0 ) <-pi ) th ( 0 ) +=2*pi;
			return th;
		}
	};

	class PMSMlin: public KalmanFull {
	protected:
		MPFpmsm &p;
		double thm;
	public:
		PMSMlin ( MPFpmsm &p0 ) :p ( p0 ) {};
		PMSMlin ( const PMSMlin &P0 ) : KalmanFull ( P0 ), p ( P0.p ), thm ( P0.thm ) {}
		BM* _copy_() const {return new PMSMlin ( *this );}

		void fillA() {
			//ia
			//using namespace p;
			A ( 0,0 ) = ( 1.0- p.Rs/p.Ls*p.dt ) ;
			A ( 0,2 ) = p.Ypm/p.Ls*p.dt* sin ( thm );
			//ib
			A ( 1,1 ) = ( 1.0- p.Rs/p.Ls*p.dt ) ;
			A ( 1,2 ) = - p.Ypm/p.Ls*p.dt* cos ( thm );
			//om
			A ( 2,0 ) = p.kp*p.p*p.p * p.Ypm/p.J*p.dt* ( -sin ( thm ) );
			A ( 2,1 ) =p.kp*p.p*p.p * p.Ypm/p.J*p.dt* ( cos ( thm ) );
			A ( 2,2 ) = 1.0;
		}

		void condition ( const vec &cond ) {
			thm = cond ( 0 );
			fillA();
		}
	};

	shared_ptr<rwtheta> rwt;

	vec dQ;
	vec dR;
public:
	void from_setting ( const Setting &set ) {
		BM::from_setting ( set ); //reads drv

		const SettingResolver& params_b=set["params"];
		const Setting& params=params_b.result;

		Rs= params["Rs"];
		Ls = params["Ls"];
		dt = 125e-6;
		Ypm=params["Fmag"];
		kp= params["kp"];
		p= params["p"];
		J = params["J"];

		UI::get ( dQ, set, "dQ", UI::compulsory );
		UI::get ( dR, set, "dR", UI::compulsory );

		mat Q = diag ( dQ ( 0,2 ) );
		mat R = diag ( dR ( 0,1 ) );

		mat A=zeros ( 3,3 );
		mat B=concat_vertical ( dt/Ls*eye ( 2 ), zeros ( 1,2 ) );
		mat C=zeros ( 2,3 );
		C ( 0,0 ) =1.0;
		C ( 1,1 ) =1.0;
		mat D= zeros ( 2,2 );

		shared_ptr<PMSMlin> kal=new PMSMlin ( *this );
		kal->set_parameters ( A,B,C,D,Q,R );
		kal->set_statistics ( zeros ( 3 ), eye ( 3 ) );
		kal->condition ( vec_1 ( 0.0 ) );
		kal->validate();

		pf = new PF;
		rwt=new rwtheta ( *this );
		pf->prior_from_set ( set );
		pf->resmethod_from_set ( set );
		pf->set_model ( rwt,new mpdf() );

		shared_ptr<RV> rv = UI::build<RV> ( set, "rv", UI::optional );
		if ( rv ) {
			set_rv ( *rv );
		}

		if ( posterior()._rv()._dsize() >0 ) {
			kal->set_rv ( posterior()._rv().subselect ( "0,1,2" ) );
			pf->set_rv ( posterior()._rv().subselect ( "3" ) );
		}

		this->set_BM ( *kal );

		string opt;
		if ( UI::get ( opt,set,"options",UI::optional ) ) {
			set_options ( opt );
		}

		validate();
	}
	void bayes ( const vec &yt, const vec &cond ) {
		const vec &mu = posterior().mean();
		const vec &Var =  posterior().variance();
		rwt->set_parameters ( mu ( 3 ),Var ( 3 ),dQ ( 3 ) );
// COPY from MPF bayes:
		int i;
		int n=pf->__w().length();
		vec &lls = pf->_lls();

// generate samples - time step
		for ( int i = 0; i < n; i++ ) {
			const vec &mu = BMs ( i )->posterior().mean();
			const vec &Var =  BMs ( i )->posterior().variance();
			rwt->set_parameters ( mu ( 2 ),Var ( 2 ),dQ ( 3 ) );

			Array<vec> &smp=pf->__samples();
			smp ( i ) = rwt->samplecond ( smp ( i ) );
			lls ( i ) = 0;
		}
// weight them - data step
#pragma parallel for
		for ( i = 0; i < n; i++ ) {
			BMs ( i ) -> bayes ( yt, concat(cond, pf->posterior()._sample ( i )) );
			lls ( i ) += BMs ( i )->_ll();
		}

		pf->bayes_weights();

		ivec ind;
		if ( pf->do_resampling() ) {
			pf->resample ( ind );

#pragma omp parallel for
			for ( i = 0; i < n; i++ ) {
				if ( ind ( i ) != i ) {//replace the current Bm by a new one
					delete BMs ( i );
					BMs ( i ) = BMs ( ind ( i ) )->_copy_(); //copy constructor
				}
			};
		}


	}
};

UIREGISTER ( MPFpmsm );



/*!@}*/
#endif //PMSMFIL_H