#include <estim/arx.h>
#include <estim/merger.h>
#include <stat/libEF.h>
#include <stat/loggers.h>
//#include <stat/merger.h>
using namespace itpp;

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

int main() {
	// Setup model
	RV y ( "{y }" );
	RV u ( "{u }" );
	RV um = u; um.t(-1);
	RV z ( "{z }" );
	RV a ("{a }");
	RV b ("{b }");
	RV c ("{c }");
	RV r ("{r }");
	
	double at = 1.5;
	double bt = 0.8;
	double ct = 0.50;
	double sig = 0.10;
	// Full system
	vec thy =vec_2 ( at,bt ); //Simulated system - zero for constant term
	vec thu =vec_2 ( at,ct ); //Simulated system - zero for constant term
	vec Thy = concat ( thy, vec_1(sig) ); //Full parameter
	vec Thu = concat ( thu, vec_1(sig) ); //Full parameter

	//ARX constructor
	mat V0 = 0.001*eye ( 3 ); V0 ( 0,0 ) = 1; //

	ARX P1 ( concat ( a, concat(b,r) ), V0, -1 );
	ARX P2 ( concat ( a, concat(c,r) ), V0, -1 );

	//Test estimation
	int ndat = 100;
	int t;

	// Logging
	dirfilelog L ( "exp/merg_2a",ndat );
	int Li_Data = L.add ( RV ( "{Y U1 U2 }" ), "" );
	int Li_LL   = L.add ( RV ( "{G M }" ), "LL" );
	int Li_P1m   = L.add ( RV ( "{a b r }" ), "P1" );
	int Li_P2m   = L.add ( RV ( "{a c r }" ), "P2" );
	L.init();

	vec Yt ( ndat );
	vec yt ( 1 );

	//Proposal
	enorm<ldmat> g0 ( a1 ); g0.set_parameters ( "1  ",mat ( "1" ) );
	egamma g1 ( r ); g1.set_parameters ( "2  ", "2" );
	enorm<ldmat> g2 ( a2 ); g2.set_parameters ( "1  ",mat ( "1" ) );

	Array<const epdf*> A ( 3 ); A ( 0 ) = &g0; A ( 1 ) =&g1; A ( 2 ) = &g2;
	eprod G0 ( A );

	for ( t=0; t<ndat; t++ ) {
		// True system
		rgrg ( 0 ) = pow ( sin ( ( t/40.0 ) *pi ),3 );
		rgrg ( 1 ) = pow ( cos ( ( t/40.0 +0.1 ) *pi ),3 );

		Yt ( t ) = thg*rgrg + sqr * NorRNG();

		// Bayes for all
		P1.bayes ( concat ( Yt ( t ),vec_1 ( rgrg ( 0 ) ) ) );
		P2.bayes ( concat ( Yt ( t ),vec_1 ( rgrg ( 1 ) ) ) );
		PG.bayes ( concat ( Yt ( t ),rgrg ) );


		// crippling PGk by substituting zeros.
		/*	ldmat &Vk=const_cast<egiw*>(PGk._e())->_V();  //PG ldmat does not like 0!
			mat fVk=PG._e()->_V().to_mat();
			fVk(1,2) = 0.0;
			fVk(2,1) = 0.0;
			Vk = ldmat(fVk);
		*/	//PGk is now krippled

		// Merge estimates
		mepdf eG1 ( P1._e() );
		mepdf eG2 ( P2._e() );
		Array<mpdf*> A ( 2 ); A ( 0 ) =&eG1;A ( 1 ) =&eG2;
		merger M ( A );
		M.set_parameters ( 10, 100,3 ); //M._Mix().set_method(QB);
		//M2.set_parameters ( 100.0, 1000,3 ); //M2._Mix().set_method(QB);
		M.merge ( &G0 );
		//M2.merge ( &G0 );

		//Likelihood
		yt ( 0 ) = Yt ( t );

//		LLs ( 0 ) = P1._e()->evalpdflog ( get_vec(Th, "1 2") );
//		LLs ( 1 ) = P2._e()->evalpdflog ( get_vec(Th, "3 2")  );
		LLs ( 0 ) = PG._e()->evalpdflog ( Th );
		LLs ( 1 ) = M._Mix().logpred ( concat ( Thj, vec_1 ( 1.0 ) ) );
//		LLs ( 3 ) = M2._Mix().logpred ( concat(Thj, vec_1(1.0)) );
		L.logit ( Li_LL, LLs ); //log-normal

		//Logger
		L.logit ( Li_Data, vec_3 ( Yt ( t ), rgrg ( 0 ), rgrg ( 1 ) ) );
		L.logit ( Li_P1m, P1._e()->mean() );
		L.logit ( Li_P2m, P2._e()->mean() );
		L.logit ( Li_Gm, PG._e()->mean() );
		L.logit ( Li_Mm, M.mean() );

		L.step ( );

		cout << "Vg: " << PG._e()->_V().to_mat() <<endl;
		vec mea = M.mean();
		cout << "Ve: " << M.variance() <<endl;
	}
	L.finalize( );
	L.itsave ( "merg_egiw.it" );
	cout << endl;
}