#include "particles.h"

namespace bdm {

using std::endl;

void PF::bayes_gensmp ( const vec &ut ) {
	if ( ut.length() > 0 ) {
		vec cond ( par->dimensionc() );
		cond.set_subvector ( par->dimension(), ut );
#pragma parallel for
		for ( int i = 0; i < n; i++ ) {
			cond.set_subvector ( 0, _samples ( i ) );
			_samples ( i ) = par->samplecond ( cond );
			lls ( i ) = 0;
		}
	} else {
#pragma parallel for
		for ( int i = 0; i < n; i++ ) {
			_samples ( i ) = par->samplecond ( _samples ( i ) );
			lls ( i ) = 0;
		}
	}
}

void PF::bayes_weights() {
//
	double mlls = max ( lls );
	// compute weights
	for ( int  i = 0; i < n; i++ ) {
		_w ( i ) *= exp ( lls ( i ) - mlls ); // multiply w by likelihood
	}

	//renormalize
	double sw = sum ( _w );
	if ( !std::isfinite ( sw ) ) {
		for ( int i = 0; i < n; i++ ) {
			if ( !std::isfinite ( _w ( i ) ) ) {
				_w ( i ) = 0;
			}
		}
		sw = sum ( _w );
		if ( !std::isfinite ( sw ) || sw == 0.0 ) {
			bdm_error ( "Particle filter is lost; no particle is good enough." );
		}
	}
	_w /= sw;
}

void PF::bayes ( const vec &yt, const vec &cond ) {
	const vec &ut = cond; //todo

	int i;
	// generate samples - time step
	bayes_gensmp ( ut );
	// weight them - data step
	for ( i = 0; i < n; i++ ) {
		lls ( i ) += obs->evallogcond ( yt, _samples ( i ) ); //+= because lls may have something from gensmp!
	}

	bayes_weights();

	if ( do_resampling() ) {
		est.resample ( resmethod );
	}

}

// void PF::set_est ( const epdf &epdf0 ) {
// 	int i;
//
// 	for ( i=0;i<n;i++ ) {
// 		_samples ( i ) = epdf0.sample();
// 	}
// }

vec MPF::mpfepdf::mean() const {
	const vec &w = pf->posterior()._w();
	vec pom = zeros ( BMs ( 0 )->posterior ().dimension() );
	//compute mean of BMs
	for ( int i = 0; i < w.length(); i++ ) {
		pom += BMs ( i )->posterior().mean() * w ( i );
	}
	return concat ( pf->posterior().mean(), pom );
}

vec MPF::mpfepdf::variance() const {
	const vec &w = pf->posterior()._w();

	vec pom = zeros ( BMs ( 0 )->posterior ().dimension() );
	vec pom2 = zeros ( BMs ( 0 )->posterior ().dimension() );
	vec mea;

	for ( int i = 0; i < w.length(); i++ ) {
		// save current mean
		mea = BMs ( i )->posterior().mean();
		pom += mea * w ( i );
		//compute variance
		pom2 += ( BMs ( i )->posterior().variance() + pow ( mea, 2 ) ) * w ( i );
	}
	return concat ( pf->posterior().variance(), pom2 - pow ( pom, 2 ) );
}

void MPF::mpfepdf::qbounds ( vec &lb, vec &ub, double perc ) const {
	//bounds on particles
	vec lbp;
	vec ubp;
	pf->posterior().qbounds ( lbp, ubp );

	//bounds on Components
	int dimC = BMs ( 0 )->posterior().dimension();
	int j;
	// temporary
	vec lbc ( dimC );
	vec ubc ( dimC );
	// minima and maxima
	vec Lbc ( dimC );
	vec Ubc ( dimC );
	Lbc = std::numeric_limits<double>::infinity();
	Ubc = -std::numeric_limits<double>::infinity();

	for ( int i = 0; i < BMs.length(); i++ ) {
		// check Coms
		BMs ( i )->posterior().qbounds ( lbc, ubc );
		//save either minima or maxima
		for ( j = 0; j < dimC; j++ ) {
			if ( lbc ( j ) < Lbc ( j ) ) {
				Lbc ( j ) = lbc ( j );
			}
			if ( ubc ( j ) > Ubc ( j ) ) {
				Ubc ( j ) = ubc ( j );
			}
		}
	}
	lb = concat ( lbp, Lbc );
	ub = concat ( ubp, Ubc );
}



void MPF::bayes ( const vec &yt, const vec &cond ) {
	// follows PF::bayes in most places!!!
	int i;
	int n = pf->__w().length();
	vec &lls = pf->_lls();
	Array<vec> &samples = pf->__samples();

	// generate samples - time step
	pf->bayes_gensmp ( vec ( 0 ) );
	// weight them - data step
#pragma parallel for
	for ( i = 0; i < n; i++ ) {
		vec bm_cond ( BMs ( i )->dimensionc() );
		this2bm.fill_cond ( yt, cond, bm_cond );
		pf2bm.filldown ( samples ( i ), bm_cond );
		BMs ( i ) -> bayes ( this2bm.pushdown ( yt ), bm_cond );
		lls ( i ) += BMs ( i )->_ll();
	}

	pf->bayes_weights();

	ivec ind;
	if ( pf->do_resampling() ) {
		pf->resample ( ind );

#pragma omp parallel for
		for ( i = 0; i < n; i++ ) {
			if ( ind ( i ) != i ) {//replace the current Bm by a new one
				delete BMs ( i );
				BMs ( i ) = (BM*) BMs ( ind ( i ) )->_copy(); //copy constructor
			}
		};
	}
};


void MPF_ARXg::bayes ( const vec &yt, const vec &cond ) {
	// follows PF::bayes in most places!!!
	int i;
	int n = pf->__w().length();
	vec &lls = pf->_lls();
	Array<vec> &samples = pf->__samples();
	
	// generate samples - time step
	for (int i =0;i<n; i++){
		mat M; chmat R;
		BMsp(i)->posterior().sample_mat(M,R);
		vec diff=R._Ch().T()*randn(samples(i).length());
		samples(i) = g->eval(samples(i)) + diff;
		////////////
//		samples(i) = cond;
		/////////
		BMsp(i)->bayes(diff);
	}
	// weight them - data step
	enorm<ldmat> ooo;
	ooo.set_parameters(zeros(2),0.1*eye(2));
	ooo.validate();
	
	#pragma parallel for
	for ( i = 0; i < n; i++ ) {
		vec tmp=yt - h->eval(samples(i));
		BMso ( i ) -> bayes ( tmp );
		lls ( i ) += BMso ( i )->_ll();
		/////
		ooo._mu()=h->eval(samples(i));
		lls(i) = ooo.evallog_nn(yt);
	}
	
	pf->bayes_weights();
	
	ivec ind;
	if ( pf->do_resampling() ) {
		pf->resample ( ind );
		
		#pragma omp parallel for
		for ( i = 0; i < n; i++ ) {
			if ( ind ( i ) != i ) {//replace the current Bm by a new one
				delete BMso ( i );
				BMso ( i ) = (ARX*) BMso ( ind ( i ) )->_copy(); //copy constructor
				delete BMsp ( i );
				BMsp ( i ) = (ARX*) BMsp ( ind ( i ) )->_copy(); //copy constructor
			}
		};
	}
};

void MPF_ARXg::from_setting(const libconfig::Setting& set) {
	BM::from_setting(set);
	
	pf = new PF;
	// prior must be set before BM
	pf->prior_from_set ( set );
	pf->resmethod_from_set ( set );
	
	string opt;
	if ( UI::get ( opt, set, "options", UI::optional ) ) {
		set_options ( opt );
	}
	
	// read functions g and h
	g=UI::build<fnc>(set,"g");
	h=UI::build<fnc>(set,"h");
	
	set_dim( g->dimension());
	dimy = h->dimension();
	
	shared_ptr<ARX> arxo=UI::build<ARX>(set,"arxo");
	shared_ptr<ARX> arxp=UI::build<ARX>(set,"arxp");
	int n = pf->__samples().length();
	BMso.set_length(n);
	BMsp.set_length(n);
	for(int i=0; i<n; i++){
		BMso(i)=arxo->_copy();
		BMsp(i)=arxp->_copy();
	}
	
	yrv = arxo->_yrv();
	//rvc = arxo->_rvc();
	
	validate();
	
}


}
//MPF::MPF:{}