#include "ekf_mm.h"
#include "qmath.h"



void ekfCh2(ekf_data *E, int16 ux, int16 uy, int16 isx, int16 isy, int16 *detS, int16 *rem)
{
	int16 t_sin,t_cos;
	int32 tmp;
	
	//x_est[0]=x_est[0];
	//               q15              + q15*q13
	(E->x_est)[1]=(((int32)(E->x_est[1])<<15)+(int32)(E->cG)*(E->x_est[0]))>>15;
	
	mmultACh(E->PSI,E->Chf,E->PSICh,2,2);
	
	givens_fast(E->PSICh,E->Q,2); // pracuje inplace
	for (int i=0;i<4;i++) E->Chf[i]=E->PSICh[i];	
	
	// implementace v PC
	t_sin=qsin(E->x_est[1]);
	t_cos=qcos(E->x_est[1]);
	
	{	
		E->C[0]=((int32)(E->cB)*t_sin)>>15;
		tmp=((int32)(E->cH)*(E->x_est[0]))>>15; // ! cH =cB with different scale!!
		E->C[1]=((int32)tmp*t_cos)>>15;
		E->C[2]=-((int32)(E->cB)*t_cos)>>15;
		E->C[3]=((int32)tmp*t_sin)>>15;
	}
	
	
	tmp=((int32)(E->cB)*(E->x_est[0]))>>15; // q15*q13 -> q13
	//             q15*q13 +          q13*q15      + q15*q13??
	E->y_est[0]=((int32)(E->cA)*(E->y_old[0])+(int32)tmp*t_sin+(int32)(E->cC)*(ux))>>15; // in q13
	E->y_est[1]=((int32)(E->cA)*(E->y_old[1])-(int32)tmp*t_cos+(int32)(E->cC)*(uy))>>15; // q13
	
	
	int16 difz[2];
	difz[0]=(isx-(E->y_est[0])); // shift to q15!!
	difz[1]=(isy-(E->y_est[1]));//<<2;
	
	E->y_old[0] = isx;
	E->y_old[1] = isy;
	
	*detS = 32767; *rem=0;
	carlson_fastC(difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem);
}

void init_ekfCh2(ekf_data *E, double Tv)
{
	// Tuning of matrix Q
	E->Q[0]=prevod(0.005,15);      // 1e-3
	E->Q[3]=prevod(0.001,15);      // 1e-3
	
	E->Chf[0]=0x1FFF;// !       // 0.05
	E->Chf[1]=E->Chf[2]=0;
	E->Chf[3]=0x1FFF;//!
		
	// Tuning of matrix R
	E->dR[0]=prevod(0.001,15);         // 0.05
	E->dR[1]=E->dR[0];
	
	// Motor model parameters 
	E->cA=prevod(1-Tv*Rs/Ls,15);
	E->cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
	E->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);
	E->cG=prevod(Tv*Wref/Thetaref,15); //in q15!
	//cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
	// cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < ===========
	E->cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 
	//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
	
	/* Init matrix PSI with permanently constant terms */
	E->PSI[0]=0x7FFF;
	E->PSI[2]=E->cG;
	E->PSI[3]=0x7FFF;
}

void ekfmm(ekf_data* E1, ekf_data* E2, int16 ux, int16 uy, int16 isx, int16 isy)
{
	int16 det1, det2, rem1, rem2;
	ekfCh2(E1, ux,uy,isx,isy,&det1,&rem1);
	ekfCh2(E2, ux,uy,isx,isy,&det2,&rem2);
	
	
}