#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 );

	vec vQ1 = "0.01:0.04:1";
	vec vQ2 = "0.01:0.04:1";

	mat LL = zeros ( vQ1.length(),  vQ2.length() );

	Ndat = Dt.cols();

	RV rx ( "{x}", "2" );
	RV ru ( "{u}", "1" );
	RV ry ( "{y}", "2" );
	//
	//
	Kalman<ldmat> KFtr;

	for ( int i = 0; i < vQ1.length(); i++ ) {

		for ( int j = 0; j < vQ2.length(); j++ ) {
			// KF with R unknown
			mat Qj = Q;
			Qj ( 0, 0 ) = vQ1 ( i );
			Qj ( 1, 1 ) = vQ2 ( j );
			KFtr.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Qj ) );
			KFtr.set_est ( mu0, ldmat ( P0 ) );

			for ( int t = 1; t < Ndat; t++ ) {
				KFtr.bayes ( Dt.get_col ( t ) );
				LL ( i, j ) += KFtr._ll();
			}
		}
	}

	it_file fou ( "testKF_QR_exh.it" );

	fou << Name ( "LL" ) << LL;
	fou << Name ( "Q1" ) << vQ1;
	fou << Name ( "Q2" ) << vQ2;
	//Exit program:
	return 0;

}