/*!
  \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 "../estim/arx_ext.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 {
	//! Introduces two new options:
	//! \li weights - meaning that all weightes will be logged
	//! \li samples - all samples will be also logged
	LOG_LEVEL(PF,weights,samples);

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<pdf> par;
	//! Observation model
	shared_ptr<pdf> 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<pdf> par0, shared_ptr<pdf> 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() ), "Incompatible input" );
		est = epdf0;
	};
	//!@}

	//! Set posterior density by sampling from epdf0
	//! 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 &yt, const vec &cond );
	//!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   = pdf_class;         // parameter evolution pdf
	observation_pdf = pdf_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<pdf> ( set, "parameter_pdf", UI::compulsory );
		obs = UI::build<pdf> ( 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_yrv ( obs->_rv() );
		rvc = 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();
		lls=zeros(n);
	}

	void validate() {
		BM::validate();
		bdm_assert ( par, "PF::parameter_pdf not specified." );
		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;
	}

	virtual double logpred ( const vec &yt ) const NOT_IMPLEMENTED(0);

	virtual epdf* epredictor() const NOT_IMPLEMENTED(NULL);
	
	virtual pdf* predictor() const NOT_IMPLEMENTED(NULL);
};
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 MPFpdf.
*/

class MPF : public BM  {
	//! Introduces new option 
	//! \li means - meaning TODO
	LOG_LEVEL(MPF,means);
protected:
	//! particle filter on non-linear variable
	shared_ptr<PF> pf;
	//! Array of Bayesian models
	Array<BM*> BMs;

	//! internal class for pdf 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;

		vec variance() const;

		void qbounds ( vec &lb, vec &ub, double perc = 0.95 ) const;

		vec sample() const NOT_IMPLEMENTED(0);

		double evallog ( const vec &val ) const NOT_IMPLEMENTED(0);		
	};

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

	//! datalink from global yt and cond (Up) to BMs yt and cond (Down)
	datalink_m2m this2bm;
	//! datalink from global yt and cond (Up) to PFs yt and cond (Down)
	datalink_m2m this2pf;
	//!datalink from PF part to BM
	datalink_part pf2bm;

public:
	//! Default constructor.
	MPF () :  jest ( pf, BMs ) {};
	//! set all parameters at once
	void set_pf ( shared_ptr<pdf> par0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) {
		if (!pf) pf=new PF;
		pf->set_model ( par0, par0 ); // <=== nasty!!!
		pf->set_parameters ( n0, rm );
		pf->set_rv(par0->_rv());
		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 ) = (BM*) BMcond0._copy();
		}
	};

	void bayes ( const vec &yt, const vec &cond );

	const epdf& posterior() const {
		return jest;
	}

	//!Access function
	const BM* _BM ( int i ) {
		return BMs ( i );
	}
	PF& _pf() {return *pf;}


	virtual double logpred ( const vec &yt ) const NOT_IMPLEMENTED(0); 
		
	virtual epdf* epredictor() const NOT_IMPLEMENTED(NULL); 
	
	virtual pdf* predictor() const NOT_IMPLEMENTED(NULL); 


	/*! configuration structure for basic PF
	\code
	BM              = BM_class;           // Bayesian filtr for analytical part of the model
	parameter_pdf   = pdf_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 ) {
		BM::from_setting( set );

		shared_ptr<pdf> par = UI::build<pdf> ( set, "parameter_pdf", UI::compulsory );

		pf = new PF;
		// prior must be set before BM
		pf->prior_from_set ( set );
		pf->resmethod_from_set ( set );
		pf->set_model ( par, par ); // too hackish!

		shared_ptr<BM> BM0 = UI::build<BM> ( set, "BM", UI::compulsory );
		set_BM ( *BM0 );

		//set drv
		//??set_yrv(concat(BM0->_yrv(),u) );
		set_yrv ( BM0->_yrv() );
		rvc = BM0->_rvc().subt ( par->_rv() );
		//find potential input - what remains in rvc when we subtract rv
		RV u = par->_rvc().subt ( par->_rv().copy_t ( -1 ) );
		rvc.add ( u );
		dimc = rvc._dsize();
		validate();
	}

	void validate() {
		BM::validate();
		try {
			pf->validate();
		} catch ( std::exception ) {
			throw UIException ( "Error in PF part of MPF:" );
		}
		jest.read_parameters();
		this2bm.set_connection ( BMs ( 0 )->_yrv(), BMs ( 0 )->_rvc(), yrv, rvc );
		this2pf.set_connection ( pf->_yrv(), pf->_rvc(), yrv, rvc );
		pf2bm.set_connection ( BMs ( 0 )->_rvc(), pf->posterior()._rv() );
	}
};
UIREGISTER ( MPF );

/*! ARXg for estimation of state-space variances
*/
class MPF_ARXg :public BM{
	protected:
	shared_ptr<PF> pf;
	//! pointer to Array of BMs
	Array<ARX*> BMso;
	//! pointer to Array of BMs
	Array<ARX*> BMsp;
	//!parameter evolution
	shared_ptr<fnc> g;
	//!observation function
	shared_ptr<fnc> h;
	
	public:
		void bayes(const vec &yt, const vec &cond );
		void from_setting(const Setting &set) ;
		void validate() {
			bdm_assert(g->dimensionc()==g->dimension(),"not supported yet");
			bdm_assert(h->dimensionc()==g->dimension(),"not supported yet");			
		}

		double logpred(const vec &cond) const NOT_IMPLEMENTED(0.0);
		epdf* epredictor() const NOT_IMPLEMENTED(NULL);
		pdf* predictor() const NOT_IMPLEMENTED(NULL);
		const epdf& posterior() const {return pf->posterior();};
		
		void log_register( logger &L, const string &prefix ){
			BM::log_register(L,prefix);
			logrec->ids.set_size ( 3 );
			logrec->ids(1)= L.add_vector(RV("Q",dimension()*dimension()), prefix+L.prefix_sep()+"Q");
			logrec->ids(2)= L.add_vector(RV("R",dimensiony()*dimensiony()), prefix+L.prefix_sep()+"R");
			
		};
		void log_write() const {
			BM::log_write();
			mat mQ=zeros(dimension(),dimension());
			mat pom=zeros(dimension(),dimension());
			mat mR=zeros(dimensiony(),dimensiony());
			mat pom2=zeros(dimensiony(),dimensiony());
			mat dum;
			const vec w=pf->posterior()._w();
			for (int i=0; i<w.length(); i++){
				BMsp(i)->posterior().mean_mat(dum,pom);
				mQ += w(i) * pom;
				BMso(i)->posterior().mean_mat(dum,pom2);
				mR += w(i) * pom2;
				
			}
			logrec->L.log_vector ( logrec->ids ( 1 ), cvectorize(mQ) );
			logrec->L.log_vector ( logrec->ids ( 2 ), cvectorize(mR) );
			
		}
};
UIREGISTER(MPF_ARXg);


}
#endif // KF_H


