#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 &dt ) {
	vec yt=dt.left(obs->dimension());
	vec ut=dt.get(obs->dimension(),dt.length()-1);
	
	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();
// 	}
// }

void MPF::bayes ( const vec &dt ) {	
	// follows PF::bayes in most places!!!	
	int i;
	int n=pf->__w().length();
	vec &lls = pf->_lls();
	
	// generate samples - time step
	pf->bayes_gensmp(vec(0));
	// weight them - data step
	#pragma parallel for
	for ( i = 0; i < n; i++ ) {
		BMs(i) -> condition(pf->posterior()._sample(i));
		BMs(i) -> bayes(dt);
		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 ) = BMs ( ind ( i ) )->_copy_(); //copy constructor
			}
		};
	}
};


}
//MPF::MPF:{}