/*!
  \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/libKF.h>
#include "fixed.h"
#include "matrix.h"
#include "reference.h"
#include "parametry_motoru.h"

using namespace bdm;

double minQ(double Q);

/*!
\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;}
};
	//! Here dt = [yt;ut] of appropriate dimensions
	void bayes ( const vec &dt );
	//!dummy!
	epdf& posterior(){return E;};
	void condition ( const vec &Q0 ) {
		
		Q[0]=prevod(minQ(Q0(0)),15);       // 0.05
		Q[5]=prevod(minQ(Q0(1)),15);
		Q[10]=prevod(minQ(Q0(2)),15);      // 1e-3
		Q[15]=prevod(minQ(Q0(3)),15);      // 1e-3

	}
};


#endif // KF_H