#include "particles.h"

namespace bdm {

using std::endl;


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 ) {

    /*	if (auxiliary){
    		...
    	}*/
    // weight them - data step
    for (int i = 0; i < n; i++ ) {
        particles(i)->bayes(yt,cond);
        lls ( i ) = particles(i)->_ll(); //+= because lls may have something from gensmp!
    }

    bayes_weights();

    if ( do_resampling() ) {
        resample ( );
    }

}

// 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 );
//
// 	// 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:{}