#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 u1 ( "{u1 }" );
	RV u2 ( "{u2 }" );
	RV uu=u1; uu.add ( u2 );

	// Full system
	vec thg ( "1 1" ); //Simulated system - zero for constant term
	double sqr=1.0;
	vec Th = concat ( thg, sqr ); //Full parameter

	// Estimated systems ARX(2)
	RV a1 ( "{a1 }" );
	RV a2 ( "{a2 }" );
	RV r ( "{r }" );
	RV all =a1; all.add ( a2 ); all.add ( r );
	RV allj =a1; allj.add ( r ); allj.add ( a2 );
	vec Thj("1 1 1");
	// Setup values

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

	ARX P1 ( concat ( a1,r ), V0, -1 );
	ARX P2 ( concat ( a2,r ), V0, -1 );
	ARX PG ( all, V0g, -1 );

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

	// Logging
	dirfilelog L ( "exp/merg_giw",1 );
	int Li_Data = L.add ( RV ( "{Y U1 U2 }" ), "" );
	int Li_LL   = L.add ( RV ( "{1 2 G M }" ), "LL" );
	int Li_Gm   = L.add ( RV ( "{a1 a2 r }" ), "G" );
	int Li_Mm   = L.add ( RV ( "{a1 r a2 }" ), "M" );
	L.init();

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

	vec LLs ( 4 );
	vec rgrg ( 2 );
	
	//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 ) *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 ) );

		// 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.0, 100,2 );
		M.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 ( 2 ) = PG._e()->evalpdflog (Th );
		LLs ( 3 ) = M._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_Gm, PG._e()->mean() );
		emix *tm =M._Mix().predictor(allj);
		L.logit ( Li_Mm, tm->mean() );
		delete tm;
		L.step ( );
	}
	L.finalize( );
	L.itsave ( "merg_egiw.it" );
}