#include <estim/kalman.h>

using namespace bdm;

//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 ) {
		bdm_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.posterior();
// 	mat Xt(2,Ndat);
// 	Xt.set_col( 0,KFep.mean() );

	//Chol
	KalmanCh KF;
	KF.set_parameters ( A, B, C, D, chmat ( R ), chmat ( Q ) );
	KF.set_statistics ( mu0, chmat ( P0 ) ); //prediction!
	const epdf& KFep = KF.posterior();
	mat Xt ( dimx, Ndat );
	Xt.set_col ( 0, KFep.mean() );

	// FULL
	KalmanFull KF2;
	KF2.set_parameters( A, B, C, D, R, Q);
	KF2.set_statistics(  mu0, P0 );
	mat Xt2 ( dimx, Ndat );
	Xt2.set_col ( 0, mu0 );


	// EKF
	shared_ptr<bilinfn> fxu = new bilinfn ( A, B );
	shared_ptr<bilinfn> hxu = new bilinfn ( C, D );
	EKFCh KFE;
	KFE.set_parameters ( fxu, hxu, Q, R );
	KFE.set_statistics ( mu0, chmat ( P0 ) );
	const epdf& KFEep = KFE.posterior();
	mat XtE ( dimx, Ndat );
	XtE.set_col ( 0, KFEep.mean() );

	//test performance of each filter
	Real_Timer tt;
	vec exec_times ( 3 ); // KF, 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++ ) {
		KF2.bayes ( Dt.get_col ( t ) );
		Xt2.set_col ( t, KF2.posterior().mean() );
	}
	exec_times ( 1 ) = 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 ( 2 ) = tt.toc();


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

}