/*!
  \file
  \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
  \author Vaclav Smidl.

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

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

#ifndef EKFfix_H
#define EKFfix_H


#include <estim/kalman.h>
#include "fixed.h"
#include "matrix.h"
#include "matrix_vs.h"
#include "reference_Q15.h"
#include "parametry_motoru.h"
#include "mpf_double.h"
#include "fast_exp.h"

using namespace bdm;

double minQ(double Q);

void mat_to_int16(const imat &M, int16 *I);
void vec_to_int16(const ivec &v, int16 *I);
void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref);

#ifdef XXX
/*!
\brief Extended Kalman Filter with full matrices in fixed point16 arithmetic

An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
*/
class EKFfixed : public BM {
public:
void init_ekf(double Tv);
void ekf(double ux, double uy, double isxd, double isyd);

/* Declaration of local functions */
void prediction(int16 *ux);
void correction(void);
void update_psi(void);

/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
 int16 Q[16]; /* matrix [4,4] */
 int16 R[4]; /* matrix [2,2] */

 int16 x_est[4];
 int16 x_pred[4];
 int16 P_pred[16]; /* matrix [4,4] */
 int16 P_est[16]; /* matrix [4,4] */
 int16 Y_mes[2];
 int16 ukalm[2];
 int16 Kalm[8]; /* matrix [5,2] */

 int16 PSI[16]; /* matrix [4,4] */

 int16 temp15a[16];

 int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane

 int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
 enorm<fsqmat> E;
 mat Ry;
  
public:
	//! Default constructor
	EKFfixed ():BM(),E(),Ry(2,2){
		int16 i;
		for(i=0;i<16;i++){Q[i]=0;}
		for(i=0;i<4;i++){R[i]=0;}

		for(i=0;i<4;i++){x_est[i]=0;}
		for(i=0;i<4;i++){x_pred[i]=0;}
		for(i=0;i<16;i++){P_pred[i]=0;}
		for(i=0;i<16;i++){P_est[i]=0;}
		P_est[0]=0x7FFF;
		P_est[5]=0x7FFF;
		P_est[10]=0x7FFF;
		P_est[15]=0x7FFF;
		for(i=0;i<2;i++){Y_mes[i]=0;}
		for(i=0;i<2;i++){ukalm[i]=0;}
		for(i=0;i<8;i++){Kalm[i]=0;}

		for(i=0;i<16;i++){PSI[i]=0;}

		set_dim(4);
		E._mu()=zeros(4);
		E._R()=zeros(4,4);
		init_ekf(0.000125);
	};
	//! Here dt = [yt;ut] of appropriate dimensions
	void bayes ( const vec &yt, const vec &ut );
	//!dummy!
	const epdf& posterior() const {return E;};
	
};

UIREGISTER(EKFfixed);

#endif

//! EKF for testing q44
class EKFtest: public EKF_UD{
	void bayes ( const vec &yt, const vec &cond ) {
		EKF_UD::bayes(yt,cond);
		vec D =  prior()._R()._D();
		
		if (D(3)>10) D(3) = 10;
		
		prior()._R().__D()=D;
	}
};
UIREGISTER(EKFtest);

/*!
\brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic

An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
*/
class EKFfixedUD : public BM {
	public:
		LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP);
		
		void init_ekf(double Tv);
		void ekf(double ux, double uy, double isxd, double isyd);
				
		/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
		int16 Q[16]; /* matrix [4,4] */
		int16 R[4]; /* matrix [2,2] */
		
		int16 x_est[4]; /* estimate and prediction */
		
		int16 PSI[16]; /* matrix [4,4] */
		int16 PSIU[16]; /* matrix PIS*U, [4,4] */
		
		int16 Uf[16]; // upper triangular of covariance (inplace)
		int16 Df[4];  // diagonal covariance
		int16 Dfold[4]; // temp of D
		int16 G[16];  // temp for bierman
		
		int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
		
		enorm<fsqmat> E;
		mat Ry;
		
	public:
		//! Default constructor
		EKFfixedUD ():BM(),E(),Ry(2,2){
			int16 i;
			for(i=0;i<16;i++){Q[i]=0;}
			for(i=0;i<4;i++){R[i]=0;}
			
			for(i=0;i<4;i++){x_est[i]=0;}
			for(i=0;i<16;i++){Uf[i]=0;}
			for(i=0;i<4;i++){Df[i]=0;}
			for(i=0;i<16;i++){G[i]=0;}
			for(i=0;i<4;i++){Dfold[i]=0;}
			
			for(i=0;i<16;i++){PSI[i]=0;}
			
			set_dim(4);
			dimy = 2;
			dimc = 2;
			E._mu()=zeros(4);
			E._R()=zeros(4,4);
			init_ekf(0.000125);
		};
		//! Here dt = [yt;ut] of appropriate dimensions
		void bayes ( const vec &yt, const vec &ut );
		//!dummy!
		const epdf& posterior() const {return E;};
		void log_register(logger &L, const string &prefix){
			BM::log_register ( L, prefix );
			
				L.add_vector ( log_level, logG, RV("G",16), prefix );
				L.add_vector ( log_level, logU, RV ("U", 16 ), prefix );
				L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
				L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
				L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
				
		};
		//void from_setting();
};

UIREGISTER(EKFfixedUD);

/*!
 * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedUD2 : public BM {
public:
	LOG_LEVEL(EKFfixedUD2,logU, logG, logD, logA, logC, logP);
	
	void init_ekf2(double Tv);
	void ekf2(double ux, double uy, double isxd, double isyd);
	
	/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
	int16 Q[4]; /* matrix [4,4] */
	int16 R[4]; /* matrix [2,2] */
	
	int16 x_est[2]; /* estimate and prediction */
	int16 y_est[2]; /* estimate and prediction */
	int16 y_old[2]; /* estimate and prediction */
	
	int16 PSI[4]; /* matrix [4,4] */
	int16 PSIU[4]; /* matrix PIS*U, [4,4] */
	int16 C[4]; /* matrix [4,4] */
	
	int16 Uf[4]; // upper triangular of covariance (inplace)
	int16 Df[2];  // diagonal covariance
	int16 Dfold[2]; // temp of D
	int16 G[4];  // temp for bierman
	
	int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
	
	enorm<fsqmat> E;
	mat Ry;
	
public:
	//! Default constructor
	EKFfixedUD2 ():BM(),E(),Ry(2,2){
		int16 i;
		for(i=0;i<4;i++){Q[i]=0;}
		for(i=0;i<4;i++){R[i]=0;}
		
		for(i=0;i<2;i++){x_est[i]=0;}
		for(i=0;i<2;i++){y_est[i]=0;}
		for(i=0;i<2;i++){y_old[i]=0;}
		for(i=0;i<4;i++){Uf[i]=0;}
		for(i=0;i<2;i++){Df[i]=0;}
		for(i=0;i<4;i++){G[i]=0;}
		for(i=0;i<2;i++){Dfold[i]=0;}
		
		for(i=0;i<4;i++){PSI[i]=0;}
		for(i=0;i<4;i++){C[i]=0;}
		
		set_dim(2);
		dimc = 2;
		dimy = 2;
		E._mu()=zeros(2);
		E._R()=zeros(2,2);
		init_ekf2(0.000125);
	};
	//! Here dt = [yt;ut] of appropriate dimensions
	void bayes ( const vec &yt, const vec &ut );
	//!dummy!
	const epdf& posterior() const {return E;};
	void log_register(logger &L, const string &prefix){
		BM::log_register ( L, prefix );
		
		L.add_vector ( log_level, logG, RV("G2",4), prefix );
		L.add_vector ( log_level, logU, RV ("U2", 4 ), prefix );
		L.add_vector ( log_level, logD, RV ("D2", 2 ), prefix );
		L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix );
		L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
		L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
		
	};
	//void from_setting();
};

UIREGISTER(EKFfixedUD2);

/*!
 * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedUD3 : public BM {
public:
	LOG_LEVEL(EKFfixedUD3,logU, logG, logD, logA, logC, logP);
	
	void init_ekf3(double Tv);
	void ekf3(double ux, double uy, double isxd, double isyd);
	
	/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
	int16 Q[9]; /* matrix [4,4] */
	int16 R[4]; /* matrix [2,2] */
	
	int16 x_est[3]; /* estimate and prediction */
	int16 y_est[2]; /* estimate and prediction */
	int16 y_old[2]; /* estimate and prediction */
	
	int16 PSI[9]; /* matrix [4,4] */
	int16 PSIU[9]; /* matrix PIS*U, [4,4] */
	int16 C[6]; /* matrix [4,4] */
	
	int16 Uf[9]; // upper triangular of covariance (inplace)
	int16 Df[3];  // diagonal covariance
	int16 Dfold[3]; // temp of D
	int16 G[9];  // temp for bierman
	
	int16 cA, cB, cC, cG, cF, cH;  // cD, cE, cF, cI ... nepouzivane
	
	enorm<fsqmat> E;
	mat Ry;
	
public:
	//! Default constructor
	EKFfixedUD3 ():BM(),E(),Ry(2,2){
		int16 i;
		for(i=0;i<9;i++){Q[i]=0;}
		for(i=0;i<4;i++){R[i]=0;}
		
		for(i=0;i<3;i++){x_est[i]=0;}
		for(i=0;i<2;i++){y_est[i]=0;}
		for(i=0;i<2;i++){y_old[i]=0;}
		for(i=0;i<9;i++){Uf[i]=0;}
		for(i=0;i<3;i++){Df[i]=0;}
		for(i=0;i<4;i++){G[i]=0;}
		for(i=0;i<3;i++){Dfold[i]=0;}
		
		for(i=0;i<9;i++){PSI[i]=0;}
		for(i=0;i<6;i++){C[i]=0;}
		
		set_dim(3);
		dimc = 2;
		dimy = 2;
		E._mu()=zeros(3);
		E._R()=zeros(3,3);
		init_ekf3(0.000125);
	};
	//! Here dt = [yt;ut] of appropriate dimensions
	void bayes ( const vec &yt, const vec &ut );
	//!dummy!
	const epdf& posterior() const {return E;};
	void log_register(logger &L, const string &prefix){
		BM::log_register ( L, prefix );		
	};
	//void from_setting();
};

UIREGISTER(EKFfixedUD3);

/*!
 * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedCh : public BM {
public:
	LOG_LEVEL(EKFfixedCh,logCh, logA, logP);
	
	void init_ekf(double Tv);
	void ekf(double ux, double uy, double isxd, double isyd);
	
	/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
	int16 Q[16]; /* matrix [4,4] */
	int16 R[4]; /* matrix [2,2] */
	
	int16 x_est[4]; /* estimate and prediction */
	
	int16 PSI[16]; /* matrix [4,4] */
	int16 PSICh[16]; /* matrix PIS*U, [4,4] */
	
	int16 Chf[16]; // upper triangular of covariance (inplace)
	
	int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
	
	enorm<chmat> E;
	mat Ry;
	
public:
	//! Default constructor
	EKFfixedCh ():BM(),E(),Ry(2,2){
		int16 i;
		for(i=0;i<16;i++){Q[i]=0;}
		for(i=0;i<4;i++){R[i]=0;}
		
		for(i=0;i<4;i++){x_est[i]=0;}
		for(i=0;i<16;i++){Chf[i]=0;}
		
		for(i=0;i<16;i++){PSI[i]=0;}
		
		set_dim(4);
		dimc = 2;
		dimy =2;
		E._mu()=zeros(4);
		E._R()=zeros(4,4);
		init_ekf(0.000125);
	};
	//! Here dt = [yt;ut] of appropriate dimensions
	void bayes ( const vec &yt, const vec &ut );
	//!dummy!
	const epdf& posterior() const {return E;};
	void log_register(logger &L, const string &prefix){
		BM::log_register ( L, prefix );
		
		L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix );
		L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
		L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
		
	};
	//void from_setting();
};

UIREGISTER(EKFfixedCh);

/*!
 * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
 * 
 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
class EKFfixedCh2 : public BM {
public:
	LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP);
	
	void init_ekf2(double Tv);
	void ekf2(double ux, double uy, double isxd, double isyd);
	
	/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
	int16 Q[4]; /* matrix [4,4] */
	int16 R[4]; /* matrix [2,2] */
	
	int16 x_est[2]; /* estimate and prediction */
	int16 y_est[2]; /* estimate and prediction */
	int16 y_old[2]; /* estimate and prediction */
	
	int16 PSI[4]; /* matrix [4,4] */
	int16 PSICh[4]; /* matrix PIS*U, [4,4] */
	int16 C[4]; /* matrix [4,4] */
	
	int16 Chf[4]; // upper triangular of covariance (inplace)
	
	int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
	
	enorm<fsqmat> E;
	mat Ry;
	
public:
	//! Default constructor
	EKFfixedCh2 ():BM(),E(),Ry(2,2){
		int16 i;
		for(i=0;i<4;i++){Q[i]=0;}
		for(i=0;i<4;i++){R[i]=0;}
		
		for(i=0;i<2;i++){x_est[i]=0;}
		for(i=0;i<2;i++){y_est[i]=0;}
		for(i=0;i<2;i++){y_old[i]=0;}
		for(i=0;i<4;i++){Chf[i]=0;}
		
		for(i=0;i<4;i++){PSI[i]=0;}
		for(i=0;i<4;i++){C[i]=0;}
		
		set_dim(2);
		dimc = 2;
		dimy = 2;
		E._mu()=zeros(2);
		E._R()=zeros(2,2);
		init_ekf2(0.000125);
	};
	//! Here dt = [yt;ut] of appropriate dimensions
	void bayes ( const vec &yt, const vec &ut );
	//!dummy!
	const epdf& posterior() const {return E;};
	void log_register(logger &L, const string &prefix){
		BM::log_register ( L, prefix );
		
		L.add_vector ( log_level, logCh, RV ("Ch2", 4 ), prefix );
		L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix );
		L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
		L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
		
	};
	//void from_setting();
};

UIREGISTER(EKFfixedCh2);


//! EKF for comparison of EKF_UD with its fixed-point16 implementation
class EKF_UDfix : public BM {
	protected:
		//! logger
		LOG_LEVEL(EKF_UDfix,logU, logG);
		//! Internal Model f(x,u)
		shared_ptr<diffbifn> pfxu;
		
		//! Observation Model h(x,u)
		shared_ptr<diffbifn> phxu;
		
		//! U part
		mat U;
		//! D part
		vec D;
				
		mat A;
		mat C;
		mat Q;
		vec R;
		
		enorm<ldmat> est;
		
		
	public:
		
		//! copy constructor duplicated 
		EKF_UDfix* _copy() const {
			return new EKF_UDfix(*this);
		}
		
		const enorm<ldmat>& posterior()const{return est;};
		
		enorm<ldmat>& prior() {
			return const_cast<enorm<ldmat>&>(posterior());
		}
		
		EKF_UDfix(){}
		
		
		EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){}
		
		//! Set nonlinear functions for mean values and covariance matrices.
		void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 );
		
		//! Here dt = [yt;ut] of appropriate dimensions
		void bayes ( const vec &yt, const vec &cond = empty_vec );
		
		void log_register ( bdm::logger& L, const string& prefix ){
			BM::log_register ( L, prefix );
			
			if ( log_level[logU] )
				L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix );
			if ( log_level[logG] )
				L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix );
			
		}
		/*! Create object from the following structure
		
		\code
		class = 'EKF_UD';
		OM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
		IM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
		dQ = [...];                             % vector containing diagonal of Q
		dR = [...];                             % vector containing diagonal of R
		--- optional fields ---
		mu0 = [...];                            % vector of statistics mu0
		dP0 = [...];                            % vector containing diagonal of P0
		-- or --
		P0 = [...];                             % full matrix P0
		--- inherited fields ---
		bdm::BM::from_setting
		\endcode
		If the optional fields are not given, they will be filled as follows:
		\code
		mu0 = [0,0,0,....];                     % empty statistics
		P0 = eye( dim );              
		\endcode
		*/
		void from_setting ( const Setting &set );
		
		void validate() {};
		// TODO dodelat void to_setting( Setting &set ) const;
		
};
UIREGISTER(EKF_UDfix);


class MPF_pmsm_red:public BM{
		double qom, qth, r;


public:
		MPF_pmsm_red(){
			dimy=2;
			dimc=2;
			qom=1e-1;
			qth=1e-6;
			r=1e-1;
		};
	void bayes ( const vec &val, const vec &cond ) {
/*		const double &isa = val(0);
		const double &isb = val(1);
		const double &usa = cond(0);
		const double &usb = cond(1);*/
		mpf_bayes((floatx)val(0),(floatx)val(1),(floatx)cond(0), (floatx)cond(1));//isa,isb,usa,usb);
	}
	
	class mp:public epdf{
		LOG_LEVEL(mp,logth);
	public:
		mp():epdf(){set_dim(3);		log_level[logth]=true;
}
		vec sample() const {return zeros(3);}
		double evallog(const vec &v) const {return 0.0;}
		vec mean() const {vec tmp(3); 
			floatx es,ec,eo;
			mpf_mean(&es, &ec, &eo); 
			tmp(0)=es;tmp(1)=ec;tmp(2)=eo;
			return tmp;
		}
		vec variance() const {return zeros(3);}
		void log_register ( bdm::logger& L, const string& prefix ) {
			epdf::log_register ( L, prefix );
			if ( log_level[logth] ) {
				int th_dim = N; // dimension - dimension of cov
				L.add_vector( log_level, logth, RV ( th_dim ), prefix );
			}
		}
		void log_write() const {
			epdf::log_write();
			if ( log_level[logth] ) {
				floatx Th[N];
				mpf_th(Th);
				vec th(N); 
				for(int i=0;i<N;i++){th(i)=Th[i];}
				log_level.store( logth, th );
			}
		}
	};
	
	mp mypdf;
	const mp& posterior() const {return mypdf;}
	
	void from_setting(const Setting &set){
	    BM::from_setting(set);
		UI::get ( log_level, set, "log_level", UI::optional );

		UI::get(qom,set,"qom",UI::optional);
		UI::get(qth,set,"qth",UI::optional);
		UI::get(r,set,"r",UI::optional);
	}
	void validate(){
		mpf_init((floatx)qom,(floatx)qth,(floatx)r);
		
	}
};
UIREGISTER(MPF_pmsm_red);

//! EKF with covariance R estimated by the VB method
class EKFvbR: public EKFfull{	
    LOG_LEVEL(EKFvbR,logR,logQ);

	//! Statistics of the R estimator
	mat PsiR;
	//! Statistics of the Q estimator
	mat PsiQ;
	//! forgetting factor
	double phi;
	//! number of VB iterations
	int niter;
	//! degrees of freedom
	double nu;
	//! stabilizing element
	mat PsiR0;
	//! stabilizing element
	mat PsiQ0;
	
	void from_setting(const Setting &set){
	    EKFfull::from_setting(set);
		if (!UI::get(phi,set,"phi",UI::optional)){
			phi = 0.99;
		}
		if (!UI::get(niter,set,"niter",UI::optional)){
			niter = 3;
		}
		PsiQ = Q;
		PsiQ0 = Q;
		PsiR = R;
		PsiR0 = R;
		nu = 3;
    }	
	void log_register ( logger &L, const string &prefix ) {
		EKFfull::log_register(L,prefix);
		L.add_vector(log_level, logR, RV("{R }",vec_1<int>(4)), prefix);
		L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(4)), prefix);
	}
	void bayes ( const vec &val, const vec &cond ) {
		vec diffx, diffy;
		mat Psi_vbQ;
		mat Psi_vbR;
		nu = phi*nu + (1-phi)*2 + 1.0;
		
		//save initial values of posterior
		vec mu0=est._mu();
		fsqmat P0=est._R();
		vec xpred = pfxu->eval(mu0,cond);
		
		for (int i=0; i<niter; i++){
			est._mu() = mu0;
			est._R() = P0;
			
			EKFfull::bayes(val,cond);

			diffy = val - fy._mu();
			Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/;
			R = Psi_vbR/(nu-2);
	
			diffx = est._mu() - xpred;
			Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx) /*+mat(est._R()) /*+ A*mat(P0)*A.T()*/;
			Q = Psi_vbQ/(nu-2);
		}
		PsiQ = Psi_vbQ;
		PsiR = Psi_vbR;
//		cout <<"==" << endl << Psi << endl << diff << endl << P0 << endl << ":" << Q;
		log_level.store(logQ, vec(Q._M()._data(),4));
		log_level.store(logR, vec(R._M()._data(),4));
		
	}
};
UIREGISTER(EKFvbR);


#endif // KF_H