/*!
  \file
  \brief Bayesian Filtering using stochastic sampling (Particle Filters)
  \author Vaclav Smidl.

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

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

#ifndef PARTICLES_H
#define PARTICLES_H


#include "../stat/exp_family.h"

namespace bdm {

/*!
* \brief Trivial particle filter with proposal density equal to parameter evolution model.

Posterior density is represented by a weighted empirical density (\c eEmp ).
*/

class PF : public BM {
protected:
	//!number of particles;
	int n;
	//!posterior density
	eEmp est;
	//! pointer into \c eEmp
	vec &_w;
	//! pointer into \c eEmp
	Array<vec> &_samples;
	//! Parameter evolution model
	shared_ptr<mpdf> par;
	//! Observation model
	shared_ptr<mpdf> obs;
	//! internal structure storing loglikelihood of predictions
	vec lls;
	
	//! which resampling method will be used
	RESAMPLING_METHOD resmethod;
	//! resampling threshold; in this case its meaning is minimum ratio of active particles
	//! For example, for 0.5 resampling is performed when the numebr of active aprticles drops belo 50%.
	double res_threshold;
	
	//! \name Options
	//!@{
	//!@}

public:
	//! \name Constructors
	//!@{
	PF ( ) : est(), _w ( est._w() ), _samples ( est._samples() ) {
	};
	
	void set_parameters (int n0, double res_th0=0.5, RESAMPLING_METHOD rm = SYSTEMATIC ) {
		n = n0;
		res_threshold = res_th0;
		resmethod = rm;
	};
	void set_model ( shared_ptr<mpdf> par0, shared_ptr<mpdf> obs0) {
		par = par0;
		obs = obs0;
		// set values for posterior
		est.set_rv(par->_rv());
	};
	void set_statistics ( const vec w0, const epdf &epdf0 ) {
		est.set_statistics ( w0, epdf0 );
	};
	void set_statistics ( const eEmp &epdf0 ) {
		bdm_assert_debug(epdf0._rv().equal(par->_rv()),"Incompatibel input");
		est=epdf0;
	};
	//!@}
	//! Set posterior density by sampling from epdf0
	//! Extends original BM::set_options by two more options:
	//! \li logweights - meaning that all weightes will be logged
	//! \li logsamples - all samples will be also logged
	void set_options ( const string &opt ) {
		BM::set_options ( opt );
	}
	//! bayes I - generate samples and add their weights to lls
	virtual void bayes_gensmp(const vec &ut);
	//! bayes II - compute weights of the 
	virtual void bayes_weights();
	//! important part of particle filtering - decide if it is time to perform resampling
	virtual bool do_resampling(){ 	
		double eff = 1.0 / ( _w * _w );
		return eff < ( res_threshold*n );
	}
	void bayes ( const vec &dt );
	//!access function
	vec& __w() { return _w; }
	//!access function
	vec& _lls() { return lls; }
	//!access function
	RESAMPLING_METHOD _resmethod() const { return resmethod; }
	//! return correctly typed posterior (covariant return)
	const eEmp& posterior() const {return est;}
	
	/*! configuration structure for basic PF
	\code
	parameter_pdf   = mpdf_class;         // parameter evolution pdf
	observation_pdf = mpdf_class;         // observation pdf
	prior           = epdf_class;         // prior probability density
	--- optional ---
	n               = 10;                 // number of particles
	resmethod       = 'systematic', or 'multinomial', or 'stratified'
										  // resampling method
	res_threshold   = 0.5;                // resample when active particles drop below 50%
	\endcode
	*/
	void from_setting(const Setting &set){
		BM::from_setting(set);
		par = UI::build<mpdf>(set,"parameter_pdf",UI::compulsory);
		obs = UI::build<mpdf>(set,"observation_pdf",UI::compulsory);
		
		prior_from_set(set);
		resmethod_from_set(set);
		// set resampling method
		//set drv
		//find potential input - what remains in rvc when we subtract rv
		RV u = par->_rvc().remove_time().subt( par->_rv() ); 
		//find potential input - what remains in rvc when we subtract x_t
		RV obs_u = obs->_rvc().remove_time().subt( par->_rv() ); 
		
		u.add(obs_u); // join both u, and check if they do not overlap

		set_drv(concat(obs->_rv(),u) );
	}
	//! auxiliary function reading parameter 'resmethod' from configuration file
	void resmethod_from_set(const Setting &set){
		string resmeth;
		if (UI::get(resmeth,set,"resmethod",UI::optional)){
			if (resmeth=="systematic") {
				resmethod= SYSTEMATIC;
			} else  {
				if (resmeth=="multinomial"){
					resmethod=MULTINOMIAL;
				} else {
					if (resmeth=="stratified"){
						resmethod= STRATIFIED;
					} else {
						bdm_error("Unknown resampling method");
					}
				}
			}
		} else {
			resmethod=SYSTEMATIC;
		};
		if(!UI::get(res_threshold, set, "res_threshold", UI::optional)){
			res_threshold=0.5;
		}
		validate();
	}
	//! load prior information from set and set internal structures accordingly
	void prior_from_set(const Setting & set){
		shared_ptr<epdf> pri = UI::build<epdf>(set,"prior",UI::compulsory);
		
		eEmp *test_emp=dynamic_cast<eEmp*>(&(*pri));
		if (test_emp) { // given pdf is sampled
			est=*test_emp;
		} else {
			//int n;
			if (!UI::get(n,set,"n",UI::optional)){n=10;}
			// sample from prior
			set_statistics(ones(n)/n, *pri);
		}
		n = est._w().length();
		//validate();
	}
	
	void validate(){
		n=_w.length();
		lls=zeros(n);
		if (par->_rv()._dsize()>0) {
			bdm_assert(par->_rv()._dsize()==est.dimension(),"Mismatch of RV and dimension of posterior" );
		}
	}
	//! resample posterior density (from outside - see MPF)
	void resample(ivec &ind){
		est.resample(ind,resmethod);
	}
	//! access function
	Array<vec>& __samples(){return _samples;}
};
UIREGISTER(PF);

/*!
\brief Marginalized Particle filter

A composition of particle filter with exact (or almost exact) bayesian models (BMs).
The Bayesian models provide marginalized predictive density. Internaly this is achieved by virtual class MPFmpdf.
*/

class MPF : public BM  {
	protected:
		//! particle filter on non-linear variable
	shared_ptr<PF> pf;
	//! Array of Bayesian models
	Array<BM*> BMs;

	//! internal class for MPDF providing composition of eEmp with external components

	class mpfepdf : public epdf  {
		//! pointer to particle filter
		shared_ptr<PF> &pf;
		//! pointer to Array of BMs
		Array<BM*> &BMs;
	public:
		//! constructor
		mpfepdf (shared_ptr<PF> &pf0, Array<BM*> &BMs0): epdf(), pf(pf0), BMs(BMs0) { };
		//! a variant of set parameters - this time, parameters are read from BMs and pf
		void read_parameters(){
			rv = concat(pf->posterior()._rv(), BMs(0)->posterior()._rv());
			dim = pf->posterior().dimension() + BMs(0)->posterior().dimension();
			bdm_assert_debug(dim == rv._dsize(), "Wrong name ");
		}
		vec 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 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 qbounds ( vec &lb, vec &ub, double perc = 0.95 ) 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 );
		}

		vec sample() const {
			bdm_error ( "Not implemented" );
			return vec();
		}

		double evallog ( const vec &val ) const {
			bdm_error ( "not implemented" );
			return 0.0;
		}
	};

	//! Density joining PF.est with conditional parts
	mpfepdf jest;

	//! Log means of BMs
	bool opt_L_mea;

public:
	//! Default constructor.
	MPF () :  jest (pf,BMs) {};
	//! set all parameters at once
	void set_parameters ( shared_ptr<mpdf> par0, shared_ptr<mpdf> obs0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) {
		pf->set_model ( par0, obs0); 
		pf->set_parameters(n0, rm );
		BMs.set_length ( n0 );
	}
	//! set a prototype of BM, copy it to as many times as there is particles in pf
	void set_BM ( const BM &BMcond0 ) {

		int n=pf->__w().length();
		BMs.set_length(n);
		// copy
		//BMcond0 .condition ( pf->posterior()._sample ( 0 ) );
		for ( int i = 0; i < n; i++ ) {
			BMs ( i ) = BMcond0._copy_();
		}
	};

	void bayes ( const vec &dt );
	const epdf& posterior() const {
		return jest;
	}
	//! Extends options understood by BM::set_options by option 
	//! \li logmeans - meaning 
	void set_options ( const string &opt ) {
		BM::set_options(opt);
		opt_L_mea = ( opt.find ( "logmeans" ) != string::npos );
	}

	//!Access function
	const BM* _BM ( int i ) {
		return BMs ( i );
	}
	
	/*! configuration structure for basic PF
	\code
	BM              = BM_class;           // Bayesian filtr for analytical part of the model
	parameter_pdf   = mpdf_class;         // transitional pdf for non-parametric part of the model
	prior           = epdf_class;         // prior probability density
	--- optional ---
	n               = 10;                 // number of particles
	resmethod       = 'systematic', or 'multinomial', or 'stratified'
										  // resampling method
	\endcode
	*/	
	void from_setting(const Setting &set){
		shared_ptr<mpdf> par = UI::build<mpdf>(set,"parameter_pdf",UI::compulsory);
		shared_ptr<mpdf> obs= new mpdf(); // not used!!

		pf = new PF;
		// rpior must be set before BM
		pf->prior_from_set(set);
		pf->resmethod_from_set(set);
		pf->set_model(par,obs);
		
		shared_ptr<BM> BM0 =UI::build<BM>(set,"BM",UI::compulsory);
		set_BM(*BM0);
		
		string opt;
		if (UI::get(opt,set,"options",UI::optional)){
			set_options(opt);
		}
		//set drv
		//find potential input - what remains in rvc when we subtract rv
		RV u = par->_rvc().remove_time().subt( par->_rv() ); 		
		set_drv(concat(BM0->_drv(),u) );
		validate();
	}
	void validate(){
		try{
		pf->validate();
		} catch (std::exception &e){
			throw UIException("Error in PF part of MPF:");
		}
		jest.read_parameters();
		for ( int i = 0; i < pf->__w().length(); i++ ) {
			BMs ( i )->condition ( pf->posterior()._sample ( i ) );
		}
	}

};
UIREGISTER(MPF);

}
#endif // KF_H