/*!
  \file
  \brief Models for synchronous electric drive using IT++ and BDM
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/

#include <itpp/itbase.h>
#include <estim/arx.h>

using namespace itpp;

vec getPsi ( int t, mat &D, mat &Q );

int main() {
	// Kalman filter
	int Ndat = 90000;
	mat Q;
	mat R;
	mat D;

	it_file itf ( "sim_var.it" );
	itf >> Name ( "Q" ) >> Q;
	itf >> Name ( "R" ) >> R;
	itf >> Name ( "D" ) >> D;

	Array<std::string> Names = "{ia ib ua ub iam ibm uam ubm iamm ibmm uamm ubmm r }";
	int rglen = Names.length();
	//Regressor
	RV rgr ( linspace ( 1,rglen ),Names,ones_i ( rglen ),zeros_i ( rglen ) );
	mat V0 = 0.0001*eye ( rglen ); V0 ( 0,0 ) *=10;
	double nu0 = rglen+1;

	//Autoregressive model
	ARX Ar ( rgr,V0,nu0 );
	epdf& ARep = Ar._epdf();

	vec Psi ( rglen );
	for ( int t=2; t<Ndat; t++ ) {
		Psi =getPsi ( t, D,Q );
		Ar.bayes ( Psi );
	}

	vec th = ARep.mean();
	th.del ( th.length()-1 ); //remove est of r
	ivec bestind = Ar.structure_est ( egiw ( rgr,V0,nu0 ) );
	cout << bestind <<endl;
	cout << th <<endl;
	
	//Reconstruction
	vec Rrec ( Ndat );
	for ( int t=2; t<Ndat; t++ ) {
		Psi =getPsi ( t, D,R );
		Rrec ( t ) = th(bestind-1)*Psi ( bestind);
	}
	it_file itfr ( "Qrec.it" );
	itfr <<Name ( "Rrec" ) <<Rrec;

	return 0;
}

vec getPsi ( int t, mat &D, mat &Q ) {
	vec Psi ( 13 );
	Psi ( 0 ) = log(exp(1)+Q ( t,0 ));

	Psi ( 1 ) = D ( t,0 );
	Psi ( 2 ) = D ( t,1 );
	Psi ( 3 ) = D ( t,2 );
	Psi ( 4 ) = D ( t,3 );

	Psi ( 5 ) = D ( t-1,0 );
	Psi ( 6 ) = D ( t-1,1 );
	Psi ( 7 ) = D ( t-1,2 );
	Psi ( 8 ) = D ( t-1,3 );

	Psi ( 9 ) = D ( t-2,0 );
	Psi ( 10 ) = D ( t-2,1 );
	Psi ( 11 ) = D ( t-2,2 );
	Psi ( 12 ) = D ( t-2,3 );
	return Psi;
}