//------------------------------------------------------------------------
//      ____                        _      _
//     / ___|____ _   _ ____   ____| |__  | |
//    | |   / ___| | | |  _  \/ ___|  _  \| |
//    | |___| |  | |_| | | | | |___| | | ||_|
//     \____|_|  \_____|_| |_|\____|_| |_|(_) Media benchmarks
//                           
//	  2006, Intel Corporation, licensed under Apache 2.0 
//
//  file :	ParticleFilterPthread.h
//  author :	Christian Bienia - cbienia@cs.princeton.edu
//				Scott Ettinger - scott.m.ettinger@intel.com
//
//  description : Posix threads parallelized version of the particle filter
//					object derived from ParticleFilter.h
//		
//  modified : 
//--------------------------------------------------------------------------


#ifndef PARTICLEFILTERPTHREAD_H
#define PARTICLEFILTERPTHREAD_H

#if defined(HAVE_CONFIG_H)
# include "config.h"
#endif

#include "ParticleFilter.h"
#include "WorkPoolPthread.h"
#include "threads/WorkerGroup.h"
#include "threads/TicketDispenser.h"
#include "threads/Barrier.h"

#undef min

//Generic particle filter class templated on model object
template<class T> 
class ParticleFilterPthread: public ParticleFilter<T>, public threads::Threadable {

	using ParticleFilter<T>:: mModel;
	using ParticleFilter<T>:: mWeights;
	using ParticleFilter<T>:: mParticles;
	using ParticleFilter<T>:: mNewParticles;
	using ParticleFilter<T>:: mBestParticle;
	using ParticleFilter<T>:: mNParticles;
	using ParticleFilter<T>:: mMinParticles;
	using ParticleFilter<T>:: mBins;
	using ParticleFilter<T>:: mRnd;
	typedef typename ParticleFilter<T>::fpType fpType;
	typedef typename ParticleFilter<T>::Vectorf Vectorf;


public:
	//constructor
	ParticleFilterPthread(WorkPoolPthread &);
	//destructor
	~ParticleFilterPthread();

	//entry function for worker threads
	void Exec(threads::thread_cmd_t, threads::thread_rank_t);

protected:
	inline bool CalcWeight(std::vector<Vectorf> &particles, int i, int rank);   //calculate weight of one particle
	virtual void CalcWeights(std::vector<Vectorf> &particles);                  //calculate particle weights based on model likelihood

	//threaded version of the base class
	void GenerateNewParticles(int k);
	
	std::vector<int> mIndex;																//list of particles to regenerate


private:
	WorkPoolPthread &workers;
	threads::Barrier *workInit;
	threads::TicketDispenser<int> particleTickets;
	
	//granularity of work unit for dynamic load balancing
	const int WORKUNIT_SIZE_PARTICLEWEIGHTS;
	const int WORKUNIT_SIZE_NEWPARTICLES;

	//work to do
	std::vector<Vectorf> *particles;
	std::vector<unsigned char> *valid;
	int annealing_parameter;
};

//constructor
template<class T>
ParticleFilterPthread<T>::ParticleFilterPthread(WorkPoolPthread &_workers) : workers(_workers), WORKUNIT_SIZE_PARTICLEWEIGHTS(4), WORKUNIT_SIZE_NEWPARTICLES(32)
{
	workInit = new threads::Barrier(workers.Size());
}

//constructor
template<class T>
ParticleFilterPthread<T>::~ParticleFilterPthread()
{
	delete workInit;
}

//thread entry function
template<class T>
void ParticleFilterPthread<T>::Exec(threads::thread_cmd_t cmd, threads::thread_rank_t rank)
{
	int ticket,i;
	
	if (cmd == workers.THREADS_CMD_PARTICLEWEIGHTS) {
		//reset dispenser, set new increment to work unit size
		if(rank == 0) {
			particleTickets.resetDispenser(WORKUNIT_SIZE_PARTICLEWEIGHTS);
		}
		workInit->Wait();
	
		ticket = particleTickets.getTicket();
		while(ticket < (int)(particles->size())) {
			//process all elements in work unit
			for(i = ticket; i < (int)(particles->size()) && i < ticket + WORKUNIT_SIZE_PARTICLEWEIGHTS; i++) {
				(*valid)[i] = CalcWeight(*particles, i, rank);
			}
			ticket = particleTickets.getTicket();
		}
	} else if(cmd == workers.THREADS_CMD_NEWPARTICLES) {
		//reset dispenser, set new increment to work unit size
		if(rank == 0) {
			particleTickets.resetDispenser(WORKUNIT_SIZE_NEWPARTICLES);
		}
		workInit->Wait();
	
		ticket = particleTickets.getTicket();
		//distribute new particles randomly according to model stdDevs
		while(ticket < mNParticles) {
			//process all elements in work unit
			for(i = ticket; i < mNParticles && i < ticket + WORKUNIT_SIZE_NEWPARTICLES; i++) {
				//add new particle for each entry in each bin distributed randomly about duplicated particle
				mNewParticles[i] = mParticles[mIndex[i]];
				AddGaussianNoise(mNewParticles[i], mModel->StdDevs()[annealing_parameter], mRnd[i]);
			}
			ticket = particleTickets.getTicket();
		}
	} else {
		//unknown command
		throw -1;
	}
}


//helper function for CalcWeights.  Calculates the weight (mWeights) for one particle
template<class T>
bool ParticleFilterPthread<T>::CalcWeight(std::vector<Vectorf> &particles, int i, int rank)
{	bool valid;
	mWeights[i] = mModel->LogLikelihood(particles[i], valid, rank);                   //compute log-likelihood weights for each particle
	return valid;
}

//calculate particle weights (mWeights) and find highest likelihood particle. 
//computes an optimal annealing factor and scales the likelihoods. 
//Also removes any particles reported as invalid by the model.
template<class T>
void ParticleFilterPthread<T>::CalcWeights(std::vector<Vectorf> &particles)
{
	std::vector<unsigned char> valid(particles.size());
	mBestParticle = 0;
	fpType total = 0, best = 0, minWeight = 1e30f, annealingFactor = 1;
	mWeights.resize(particles.size());
	uint i = 0;

	ParticleFilterPthread<T>::particles = &particles;
	ParticleFilterPthread<T>::valid = &valid;
	
	//signal to workers that work is available
	workers.SendCmd(workers.THREADS_CMD_PARTICLEWEIGHTS);

	i = 0;
	while(i < particles.size())
	{	if(!valid[i])                                                                //if not valid(model prior), remove the particle from the list
		{	particles[i] = particles[particles.size() - 1];
			mWeights[i] = mWeights[particles.size() - 1];
			valid[i] = valid[valid.size() - 1];
			particles.pop_back(); mWeights.pop_back(); valid.pop_back();
		}
		else
			minWeight = std::min(mWeights[i++], minWeight);                          //find minimum weight
	}
	if((int)particles.size() < mMinParticles) return;                                //bail out if not enough valid particles
	mWeights -= minWeight;                                                           //shift weights to zero for numerical stability
	if(mModel->StdDevs().size() > 1) 
		annealingFactor = BetaAnnealingFactor(mWeights, 0.5f);                       //calculate annealing factor if more than 1 step
	for(i = 0; i < mWeights.size(); i++)
	{	double wa = annealingFactor * mWeights[i];
		mWeights[i] = (float)exp(wa);                                                //exponentiate log-likelihoods scaled by annealing factor
		total += mWeights[i];                                                        //save sum of all weights
		if(i == 0 || mWeights[i] > best)                                             //find highest likelihood particle
		{	best = mWeights[i];
			mBestParticle = i;
		}
	}
	mWeights *= (fpType)1.0 / total;                                                 //normalize weights
}

//generate new particles distributed with std deviation given by the model annealing parameter
template<class T> 
void ParticleFilterPthread<T>::GenerateNewParticles(int k)
{	int p = 0;
	mNewParticles.resize(mNParticles);
	mIndex.resize(mNParticles);
	for(int i = 0; i < (int)mBins.size(); i++)										
		for(uint j = 0; j < mBins[i]; j++)                                           //index particles to be regenerated
			mIndex[p++] = i;
			
	ParticleFilterPthread<T>::annealing_parameter = k;
	//signal to workers that work is available
	workers.SendCmd(workers.THREADS_CMD_NEWPARTICLES);
}

#endif