#include <itpp/itbase.h>
#include <estim/libKF.h>

using namespace itpp;

//These lines are needed for use of cout and endl
using std::cout;
using std::endl;

int main() {
	// Kalman filter
	mat A, B,C,D,R,Q,P0;
	vec mu0;
	mat Mu0;;
	// input from Matlab
	it_file fin( "testKF.it" );

	mat Dt;
	int Ndat;

	bool xxx= fin.seek( "d" );
	if (!xxx){ it_error("testKF.it not found");}
	fin >>Dt;
	fin.seek( "A" ); 
	fin >> A;
	fin.seek( "B" ); 
	fin >> B;
	fin.seek( "C" ); 
	fin >> C;
	fin.seek( "D" ); 
	fin >> D;
	fin.seek( "R" ); 
	fin >> R;
	fin.seek( "Q" ); fin >> Q;
	fin.seek( "P0" ); fin >> P0;
	fin.seek( "mu0" ); fin >> Mu0; 
	mu0=Mu0.get_col(0);
	
	Ndat = 10;//Dt.cols();
	int dimx = A.rows();
	
	// Prepare for Kalman filters in BDM:
	RV rx("{x }",vec_1(A.cols()));
	RV ru("{u }",vec_1(B.cols()));
	RV ry("{y }",vec_1(C.rows()));
	
// 	// LDMAT
// 	Kalman<ldmat> KF(rx,ry,ru);
// 	KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
// 	KF.set_est(mu0,ldmat(P0) );
// 	epdf& KFep = KF._epdf();
// 	mat Xt(2,Ndat);	
// 	Xt.set_col( 0,KFep.mean() );

	//Chol
	KalmanCh KF(rx,ry,ru);
	KF.set_parameters(A,B,C,D,chmat(R),chmat(Q));
	KF.set_est(mu0,chmat(P0) ); //prediction!
	epdf& KFep = KF._epdf();
 	mat Xt(dimx,Ndat);	
 	Xt.set_col( 0,KFep.mean() );
	
	//	
	// FSQMAT
	Kalman<ldmat> KFf(rx,ry,ru);
	KFf.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
	KFf.set_est(mu0,ldmat(P0) );
	epdf& KFfep = KFf._epdf();
	mat Xtf(dimx,Ndat);	
	Xtf.set_col( 0,KFfep.mean() );
	
	// FULL
	KalmanFull KF2( A,B,C,D,R,Q,P0,mu0 );
	mat Xt2(dimx,Ndat);	
	Xt2.set_col( 0,mu0);

	
	// EKF
	bilinfn fxu(rx,ru,A,B);
	bilinfn hxu(rx,ru,C,D);
	EKFCh KFE(rx,ry,ru);
	KFE.set_parameters(&fxu,&hxu,Q,R);
	KFE.set_est(mu0,chmat(P0));
	epdf& KFEep = KFE._epdf();
	mat XtE(dimx,Ndat);	
	XtE.set_col( 0,KFEep.mean() );

	//test performance of each filter
	Real_Timer tt;
	vec exec_times(4); // KF, KFf KF2, KFE
	
	tt.tic();
	for ( int t=1;t<Ndat;t++ ) {
		KF.bayes( Dt.get_col( t ));
		Xt.set_col( t,KFep.mean() );
	}
	exec_times(0) = tt.toc();
	
	tt.tic();
	for ( int t=1;t<Ndat;t++ ) {
		KFf.bayes( Dt.get_col( t ));
		Xtf.set_col( t,KFfep.mean() );
	}
	exec_times(1) = tt.toc();

	tt.tic();
	for ( int t=1;t<Ndat;t++ ) {
		KF2.bayes( Dt.get_col( t ));
		Xt2.set_col( t,KF2.mu);
	}
	exec_times(2) = tt.toc();

	tt.tic();
	for ( int t=1;t<Ndat;t++ ) {
		KFE.bayes( Dt.get_col( t ));
		XtE.set_col( t,KFEep.mean() );
	}
	exec_times(3) = tt.toc();


	it_file fou( "testKF_res.it" );
	fou << Name("xth") << Xt;
	fou << Name("xthf") << Xtf;
	fou << Name("xth2") << Xt2;
	fou << Name("xthE") << XtE;
	fou << Name("exec_times") << exec_times;
	//Exit program:
	return 0;

}