#include "ekf_example/matrix_vs.h"
#include "ekf_example/ekf_obj.h"
int main(){
	int 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 (int i=0; i<5;i++) {
		for (int 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);

		int PSI[25];
		int 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};
		int 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};
		int 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};
		int Df[5]={0,0,0,0,0};
		int Dfold[5]={0,0,0,0,0};
		
		int multip=1<<15;
		
	/////////// COPY
	imat Af=round_i(A*multip);
	mat_to_int(Af, PSI);
	mat_to_int(round_i(U*multip),Uf);		
	vec_to_int(round_i(D*multip), Df); 
	int Qf[25];
	mat_to_int(round_i(Q*multip), Qf);
	int Rf[2];
	vec_to_int(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;*/
	
	imat PUcmp(PSIU,5,5);
	cout << "Delta PSI: " << round_i(PhiU*multip-2*PUcmp) <<endl;
	
	mat_to_int(round_i(PhiU*multip/2),PSIU); //<< make is same

/////////// Test Thorton:
	int dim=5;double sigma; int 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;

	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;
	
	// synchronize
	mat_to_int(round_i(U*multip),Uf);		
	vec_to_int(round_i(D*multip), Df); 
	

	vec	ydif = 2*randu(2)-1;
	vec	xp = 2*randu(5)-1;
	
	int difz[2];
	vec_to_int(round_i(ydif*multip), difz);
	int xf[5];
	vec_to_int(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;
	
	
	int xf_old[5];
	vec_to_int(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 (int 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;	
}