#include <itpp/itbase.h>
#include "libKF.h"
#include "libDC.h"

using namespace itpp;

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

int main() {

	mat A="1 -0.5; 1 0";
	mat B="1;0";
	mat C="1 0";
	mat D="0";
	ldmat R=ldmat("0.1");
	ldmat Q=ldmat("0.2 0 ; 0 0.02");

	ldmat P0=Q; P0*=1000.0; //increase P0
	vec mu0="0 0";

	Kalman<ldmat> KF( A,B,C,D,R,Q,P0,mu0 );
//	cout << KF;

	it_file fin( "matlab/testKF.it" );
	mat Dt;

	fin >> Name( "d" ) >> Dt;
	int Ndat = Dt.cols();
	mat Xt=zeros( 2,Ndat );
	

	Xt.set_col( 0,KF.mu );
	for ( int t=1;t<Ndat;t++ ) {
		KF.bayes( Dt.get_col( t ));
		Xt.set_col(t,KF.mu);
//		Kmu = KF.mu;
//		cout <<  "t:" <<t<< "  " << dt<<"  "<<Kmu <<endl;

	}

	it_file fou( "matlab/testKF_res.it" );
	fou << Name("xth") << Xt;
	//Exit program:
	return 0;

}