#include <estim/kalman.h>
#include <estim/particles.h>

using namespace bdm;

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

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

	mat Dt, XQRt, eR, eQ;
	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 = Dt.cols();
	XQRt = zeros ( 5, Ndat );
	mat Xt = zeros ( 2, Ndat );

//	cout << KF;
	RV rx ( "{x }", "2" );
	RV ru ( "{u }", "1" );
	RV ry ( "{y }", "1" );
	RV rQR ( "{Q,R }", "3" );
	//
	KFcondQR KF;
	// KF with R unknown
	KF.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Q ) );
	KF.set_est ( mu0, ldmat ( P0 ) );
	//
	Kalman<ldmat> KFtr;
	// KF with R unknown
	KFtr.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Q ) );
	KFtr.set_est ( mu0, ldmat ( P0 ) );

	mgamma evolQR;
	evolQR.set_parameters ( 10.0, "1 1 1" ); //sigma = 1/10 mu

	MPF<KFcondQR> KF_QR;
	KF_QR.set_parameters ( &evolQR, &evolQR, 100 );
	evolQR.condition ( "1 1 1" );
	egamma kfinit=egamma("10 10 10","1 1 1");
	KF_QR.set_statistics ( egamma("10 10 10","1 1 1"), &KF );
	const epdf& mpost = KF_QR.posterior();
	const epdf& mposttr = KFtr.posterior();

	XQRt.set_col ( 0, mpost.mean() );
	Xt.set_col ( 0, mposttr.mean() );
	for ( int t = 1; t < Ndat; t++ ) {
		KF_QR.bayes ( Dt.get_col ( t ) );
		KFtr.bayes ( Dt.get_col ( t ) );

		XQRt.set_col ( t, mpost.mean() );
		Xt.set_col ( t, mposttr.mean() );
	}

	it_file fou ( "testKF_QR_res.it" );
	fou << Name ( "xqrth" ) << XQRt;
	fou << Name ( "xth" ) << Xt;
	//Exit program:
	return 0;

}