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

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

  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.h"
#include "parametry_motoru.h"

using namespace bdm;

double minQ(double Q);

void mat_to_int(const imat &M, int *I);
void vec_to_int(const ivec &v, int *I);

/*!
\brief Extended Kalman Filter with full matrices in fixed point 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(int *ux);
void correction(void);
void update_psi(void);

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

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

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

 int temp15a[16];

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

 long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
 enorm<fsqmat> E;
 mat Ry;
  
public:
	//! Default constructor
	EKFfixed ():BM(),E(),Ry(2,2){
		int 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);

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

An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
*/
class EKFfixedUD : public BM {
	public:
		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?*/
		int Q[16]; /* matrix [4,4] */
		int R[4]; /* matrix [2,2] */
		
		int x_est[4]; /* estimate and prediction */
		
		int PSI[16]; /* matrix [4,4] */
		int PSIU[16]; /* matrix PIS*U, [4,4] */
		
		int Uf[16]; // upper triangular of covariance (inplace)
		int Df[4];  // diagonal covariance
		int Dfold[4]; // temp of D
		int G[16];  // temp for bierman
		
		int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
		
		enorm<fsqmat> E;
		mat Ry;
		
	public:
		//! Default constructor
		EKFfixedUD ():BM(),E(),Ry(2,2){
			int 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);
			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(EKFfixedUD);

//! EKF for comparison of EKF_UD with its fixed-point 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;
		
		int Uf[25];
		int Df[5];
		int Dfold[5];
		
		mat A;
		mat C;
		mat Q;
		vec R;
		
		int PSI[25];
		int PSIU[25];
		int Gf[25];

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




#endif // KF_H