#include "ekf_example/matrix_vs.h"
#include "ekf_example/ekf_obj.h"
int main(){
	int16 i;
	mat A = 0.99*eye(5);
	A(0.3) = 0.06;
	A(0,2) = 0.01;
	A(1,2) = 0.01;
	A(1,3) = -0.07;
	A(3,2) = 0.0001;

	RNG_randomize();
	
	mat U=eye(5);
	for (int16 i=0; i<5;i++) {
		for (int16 j=i+1; j<5;j++) U(i,j)=2*randu(1)(0)-1;
	}
	mat Q = diag(vec(" 0.2000 0.3000 0.4000 0.5000 0.6"));
	vec R = vec(" 0.2000 0.3000");
	
		vec D = randu(5)*0.9;
		vec xref = ones(5);

		int16 PSI[25];
		int16 PSIU[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
		int16 Uf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
		int16 Gf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
		int16 Df[5]={0,0,0,0,0};
		int16 Dfold[5]={0,0,0,0,0};
		
		int16 multip=1<<15;
		
	/////////// COPY
	imat Af=round_i(A*multip);
	mat_to_int16(Af, PSI);
	mat_to_int16(round_i(U*multip),Uf);		
	vec_to_int16(round_i(D*multip), Df); 
	int16 Qf[25];
	mat_to_int16(round_i(Q*multip), Qf);
	int16 Rf[2];
	vec_to_int16(round_i(R*multip), Rf);
	
	
	////////////// Test mmultAU
	mmultAU(PSI,Uf,PSIU,5,5);	
	
	mat PhiU =A*U;
/*	cout << "A*U" << round_i(PhiU*multip) <<endl;
	cout << "PSIU: "; for (i=0; i<25;i++) cout << PSIU[i] << ","; cout <<endl;*/
	
	Mat<int16> PUcmp(PSIU,5,5);
//	cout << "Delta PSI: " << round_i(PhiU*multip-(1<<(15-qAU))*PUcmp) <<endl;
	
	mat_to_int16(round_i(PhiU*multip/(1<<(15-qAU))),PSIU); //<< make is same

/////////// Test Thorton:
	int16 dim=5;double sigma; int16 j,k; 
	vec Din = D;
	mat G=eye(5);
	for (i=dim-1; i>=0;i--){
		sigma = 0.0; 
		for (j=0; j<dim; j++) {
			sigma += PhiU(i,j)*PhiU(i,j) *Din(j); 
			sigma += G(i,j)*G(i,j) * Q(j,j); 
		}
		D(i) = sigma; 
		for (j=0;j<i;j++){ 
			sigma = 0.0; 
			for (k=0;k<dim;k++){ 
				sigma += PhiU(i,k)*Din(k)*PhiU(j,k); 
			}
			for (k=0;k<dim;k++){ 
				sigma += G(i,k)*Q(k,k)*G(j,k); 
			}
			//
			U(j,i) = sigma/D(i); 
			for (k=0;k<dim;k++){ 
				PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); 
			}
			for (k=0;k<dim;k++){ 
				G(j,k) = G(j,k) - U(j,i)*G(i,k); 
			}
		}
	}
	
	
	//thorton_fast(Uf,Df,PSIU,Qf,Gf,Dfold,5);
	thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);

	/////// disp
	
	cout << endl<<"after thorton " <<endl;
// 	cout << "U: " << round_i(U*multip) << endl;
// 	cout << "D: " << round_i(D*multip) << endl;
// 	cout << "Uf: "; for (i=0; i<25;i++) {cout << Uf[i] << ","; ((i+1)%5==0)? cout << endl:cout<<""; }
// 	cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;

	Mat<int16> Ucmp(Uf,5,5);
	Vec<int16> Dcmp(Df,5);
	cout << "Delta U: " << round_i(U*multip-Ucmp) << endl;
	cout << "Delat D: " << round_i(D*multip-Dcmp) << endl;
	
	// synchronize
	mat_to_int16(round_i(U*multip),Uf);		
	vec_to_int16(round_i(D*multip), Df); 
	

	vec	ydif = 2*randu(2)-1;
	vec	xp = 2*randu(5)-1;
	
	int16 difz[2];
	vec_to_int16(round_i(ydif*multip), difz);
	int16 xf[5];
	vec_to_int16(round_i(xp*multip), xf);

	cout << "x: "<< round_i(xp*multip) <<endl;
	cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
	
	
	int16 xf_old[5];
	vec_to_int16(ivec(xf,5),xf_old);
	
	/////// Test bierman
	double dz,alpha,gamma,beta,lambda;
	vec a;
	vec b;
	mat C = zeros(2,5);
	C(0,0)=1.0;C(1,1)=1.0;
	for (int16 iy=0; iy<2; iy++){
		a     = U.T()*C.get_row(iy);    // a is not modified, but 
		b     = elem_mult(D,a);    			 // b is modified to become unscaled Kalman gain. 
		dz    = ydif(iy); 
		
		alpha = R(iy); 
		gamma = 1/alpha; 
		for (j=0;j<dim;j++){
			beta   = alpha; 
			alpha  = alpha + a(j)*b(j); 
			lambda = -a(j)*gamma; 
			gamma  = 1.0/alpha; 
			D(j) = beta*gamma*D(j); 
			
			// 			cout << "a: " << alpha << "g: " << gamma << endl;
			for (i=0;i<j;i++){
				beta   = U(i,j); 
				U(i,j) = beta + b(i)*lambda; 
				b(i)   = b(i) + b(j)*beta; 
			}
		}
		double dzs = gamma*dz;  // apply scaling to innovations 
		xp   = xp + dzs*b; // multiply by unscaled Kalman gain 
		
		//cout << "Ub: " << U << endl;
		//cout << "Db: " << D << endl <<endl;
		
	}
	
	bierman_fast(difz,xf, Uf, Df, Rf, 2, 5);
	cout << endl<<"after Bierman" <<endl;
/*	cout << "U: " << round_i(U*multip) << endl;
	cout << "D: " << round_i(D*multip) << endl;
	cout << "Uf: "; for (i=0; i<25;i++) {cout << Uf[i] << ","; ((i+1)%5==0)? cout << endl:cout<<""; }
	cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;*/
	cout << "x: "<< round_i(xp*multip) <<endl;
	cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;

	{
	imat Ucmp(Uf,5,5);
	ivec Dcmp(Df,5);
	cout << "Delta U: " << round_i(U*multip-Ucmp) << endl;
	cout << "Delat D: " << round_i(D*multip-Dcmp) << endl;
	}
	
return 0;	
}