#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() {


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

	mat Dt, Xt,Xt2,XtE;
	int Ndat;

	bool xxx= fin.seek( "d" );
	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();
	Xt=zeros( 2,Ndat );
	Xt2=zeros( 2,Ndat );
	XtE=zeros( 2,Ndat );

	Kalman<fsqmat> KF( A,B,C,D,fsqmat(R),fsqmat(Q),fsqmat(P0),mu0 );
//	cout << KF;
	KalmanFull KF2( A,B,C,D,R,Q,P0,mu0 );
	RV rx("1","{x}","2","0","0");
	RV ru("2","{u}","1","0","0");
	RV ry("3","{y}","1","0","0");
	bilinfn fxu(rx,ru,A,B);
	bilinfn hxu(rx,ru,C,D);
	EKF<fsqmat> KFE(&fxu,&hxu,Q,R,mu0,P0);

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

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

}