#include <itpp/itbase.h>
#include <estim/libKF.h>
#include <estim/libPF.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;// read from matlab
	// input from Matlab
	it_file fin( "testKF.it" );

	mat Dt, XRt,eR,eQ;
	int Ndat;

	bool xxx= fin.seek( "d" );
	if (!xxx){ it_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();
	XRt=zeros( 3,Ndat );

//	cout << KF;
	RV rx("1","{x}","2","0");
	RV ru("2","{u}","1","0");
	RV ry("3","{y}","1","0");
	RV rR("4","{R}","1","0");
	//
	KFcondR KF(rx,ry,ru,rR);
	Kalman<fsqmat> KFtr(rx,ry,ru);
	Kalman<fsqmat> KFtr2(rx,ry,ru);
	//correct KF
	KFtr.set_parameters(A,B,C,D,fsqmat(R),fsqmat(Q));
	KFtr.set_est(mu0,fsqmat(P0) );
	// KF with lower R
	mat Q2=Q/1.0;
	mat R2=R/20.0;
	KFtr2.set_parameters(A,B,C,D,fsqmat(R2),fsqmat(Q2));
	KFtr2.set_est(mu0,fsqmat(P0) );
	// KF with R unknown
	KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
	KF.set_est(mu0,ldmat(P0) );
	//
	mgamma evolR(rR,rR);
	evolR.set_parameters(10.0); //sigma = 1/10 mu
	
	MPF<KFcondR > KF_R(rx,rR,evolR,evolR,10,KF);
	evolR.condition("0.1");
	epdf& pfinit=evolR._epdf();
	KF_R.set_est(pfinit);
	epdf& mpost=KF_R._epdf();

	cout << mpost.mean()<<endl;

	XRt.set_col( 0,mpost.mean());
	double ll1=0.0;
	double ll2=0.0;
	for ( int t=1;t<Ndat;t++ ) {
//		KF_R.bayes( Dt.get_col( t ));
		KFtr.bayes( Dt.get_col( t ));
		KFtr2.bayes( Dt.get_col( t ));
		
		ll1+=KFtr._ll();
		ll2+=KFtr2._ll();
		
		XRt.set_col(t,mpost.mean());
	}
	
	cout << ll1 << "  " << ll2 <<endl;

	it_file fou( "testKF_R_res.it" );
	fou << Name("xqrth") << XRt;
	//Exit program:
	return 0;

}