#include <estim/kalman.h>

#include "ekf_obj.h"
#include "../simulator.h"

double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};

///////////////
void EKFfixed::bayes(const vec &yt, const vec &ut){
	ekf(yt(0),yt(1),ut(0),ut(1));
	
	vec xhat(4);	
	//UGLY HACK!!! reliance on a predictor!!
	xhat(0)=zprevod(x_est[0],Qm)*Iref;
	xhat(1)=zprevod(x_est[1],Qm)*Iref;
	xhat(2)=zprevod(x_est[2],Qm)*Wref;
	xhat(3)=zprevod(x_est[3],15)*Thetaref;
	
	E.set_mu(xhat);
	
	if ( BM::evalll ) {
		//from enorm
		vec xdif(x,4);//first 4 of x
		//UGLY HACK!!! reliance on a predictor!!
/*		xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref;
		xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref;
		xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref;
		xdif(3)=x[3]-zprevod(x_pred[3],15);*/
		
		xdif -=xhat; //(xdif=x-xhat)
		
		mat Pfull(4,4);
		double* Pp=Pfull._data();
		for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
		
		E._R()._M()=Pfull;
		
		
		BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );
	}
};


void EKFfixed::update_psi(void)
{
  int t_sin,t_cos,tmp;

  // implementace v PC
  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);

  PSI[2]=((long)cB*t_sin)>>15;
  tmp=((long)cH*x_est[2])>>15;
  PSI[3]=((long)tmp*t_cos)>>15;
  PSI[6]=-((long)cB*t_cos)>>15;
  PSI[7]=((long)tmp*t_sin)>>15;
}


void EKFfixed::prediction(int *ux)
{
  int t_sin,t_cos, tmp;

  // implementace v PC
  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);

  tmp=((long)cB*x_est[2])>>15;
  x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15;
  x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15;
  x_pred[2]=x_est[2];
  x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;

  update_psi();

  mmult(PSI,P_est,temp15a,3,3,3);
//  mtrans(PSI,temp15b,5,5);
  mmultt(temp15a,PSI,P_pred,3,3,3);
  maddD(P_pred,Q,3,3);
}

void EKFfixed::correction(void)
{
  int Y_error[2];
  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */

  choice_P(P_pred,temp15a,3);
  maddD(temp15a,R,1,1);
  minv2(temp15a,temp30a);
  	Ry(0,0)=zprevod(temp15a[0],15);
  	Ry(0,1)=zprevod(temp15a[1],15);
  	Ry(1,0)=zprevod(temp15a[2],15);
  	Ry(1,1)=zprevod(temp15a[3],15);
  
  mmultDr(P_pred,temp15a,3,3,1,1);
  mmult1530(temp15a,temp30a,Kalm,3,1,1);


  /* estimate the state system */
  choice_x(x_pred, temp15a);
  msub(Y_mes,temp15a,Y_error,1,0);
  mmult(Kalm,Y_error,temp15a,3,1,0);
  madd(x_pred,temp15a,x_est,3,0);

  /* matrix of covariances - version without MMULTDL() */

/* Version with MMULTDL() */
  mmultDl(P_pred,temp15a,1,3,3,1);

  mmult(Kalm,temp15a,P_est,3,1,3);
  msub(P_pred,P_est,P_est,3,3);
/* END */
}


void EKFfixed::ekf(double ux, double uy, double isxd, double isyd)
{
  // vypocet napeti v systemu (x,y)
  ukalm[0]=prevod(ux/Uref,Qm);
  ukalm[1]=prevod(uy/Uref,Qm);

  // zadani mereni
  Y_mes[0]=prevod(isxd/Iref,Qm);
  Y_mes[1]=prevod(isyd/Iref,Qm);

  ////// vlastni rutina EKF /////////////////////////
  prediction(ukalm);
  correction();

  // navrat estimovanych hodnot regulatoru
  vec& mu = E._mu();
  (mu)(0)=zprevod(x_est[0],Qm)*Iref;
  (mu)(1)=zprevod(x_est[1],Qm)*Iref;
  (mu)(2)=zprevod(x_est[2],Qm)*Wref;
  (mu)(3)=zprevod(x_est[3],15)*Thetaref;
}

void EKFfixed::init_ekf(double Tv)
{
  // Tuning of matrix Q
  Q[0]=prevod(.01,15);       // 0.05
  Q[5]=Q[0];
  Q[10]=prevod(0.0001,15);      // 1e-3
  Q[15]=prevod(0.0001,15);      // 1e-3

  // Tuning of matrix R
  R[0]=prevod(0.05,15);         // 0.05
  R[3]=R[0];

  // Motor model parameters 
  cA=prevod(1-Tv*Rs/Ls,15);
  cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
  cC=prevod(Tv/Ls/Iref*Uref,15);
//  cD=prevod(1-Tv*Bf/J,15);
//  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
//  cF=prevod(p*Tv*Mref/J/Wref,15);
  cG=prevod(Tv*Wref*4/Thetaref,15);
  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);

  /* Init matrix PSI with permanently constant terms */
  PSI[0]=cA;
  PSI[5]=PSI[0];
  PSI[10]=0x7FFF;
  PSI[14]=cG;
  PSI[15]=0x7FFF;
}