#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 = 0.1*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);

		mat Ch = U*diag(sqrt(D));
		
		int PSI[25];
		int PSICh[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 Chf[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 Cf[10]={0,0,0,0,0, 0,0,0,0,0};
		int multip=1<<15;
		
	/////////// COPY
	imat Af=round_i(A*multip);
	mat_to_int16(Af, PSI);
	mat_to_int16(round_i(Ch*multip),Chf);		
	int Qf[25];
	mat_to_int16(round_i(sqrt(Q)*multip), Qf);
	int Rf[2];
	vec_to_int16(round_i(R*multip), Rf);
	
	
	////////////// Test mmultAU
	mmultACh(PSI,Chf,PSICh,5,5);	
	
	mat PhiCh =round(A*32768)*round(Ch*32768)/32768/32768;
/*	cout << "A*U" << round_i(PhiU*multip) <<endl;
	cout << "PSIU: "; for (i=0; i<25;i++) cout << PSIU[i] << ","; cout <<endl;*/
	
	imat PChcmp(PSICh,5,5);
	cout << "Delta PSI: " << round_i(PhiCh*multip-PChcmp) <<endl;
	
	mat_to_int16(round_i(PhiCh*multip),PSICh); //<< make is same

	mat PhiU= A*U;
/////////// 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); 
			}
		}
	}

cout << "PSICh: " << imat(PSICh,5,5) << endl;
cout << "Qf: " << imat(Qf,5,5) << endl;

Ch = U*diag(sqrt(D));

	
	householder(PSICh,Qf,5);
//	givens(PSICh,Qf,5);

	/////// disp
	
	cout << endl<<"after householder " <<endl;

	imat Chcmp(PSICh,5,5);
	cout << "Ch(UD): " << round_i(Ch*multip) << endl;
	cout << "Ch: " << (Chcmp) << endl;
	cout << "Delta Ch: " << round_i(Ch*multip-Chcmp) << endl;
	
	// synchronize
	vec d=diag(Ch*Ch.T());
	mat Ch2 = 0.9*diag(1./sqrt(d))*Ch;
	mat_to_int16(round_i(Ch2*multip),Chf);		

	D = pow(diag(Ch2),2);
	U = sqrt(diag(1./D));
	U = Ch2*U;
		
	vec	ydif = 2*randu(2)-1;
	vec	xp = 2*randu(5)-1;
	
	int difz[2];
	vec_to_int16(round_i(ydif*multip), difz);
	int 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;
	
	
	int 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)=.2;C(1,1)=0.4;C(0,1)=0.3;
	
	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;
		
	}
	
	Ch = U*diag(sqrt(D));
	mat_to_int16(round_i(C*multip),Cf);		

	cout << "bef Carlson: " << imat(Chf,5,5) << endl;

	carlson_fastC(difz,xf, Chf, Cf,Rf, 2, 5);
	
	cout << endl<<"after Carlson" <<endl;
	cout << "x: "<< round_i(xp*multip) <<endl;
	cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;

	{
		imat Chcmp(Chf,5,5);
		cout << "Delta Ch: " << round_i(Ch*multip-Chcmp) << endl;
		cout << "Delta Ch: " << Chcmp << endl;
	}
	
return 0;	
}