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

#ifndef PARTICLEFILTEROMP_H
#define PARTICLEFILTEROMP_H

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

#include <omp.h>
#include "ParticleFilter.h"

template<class T> 
class ParticleFilterOMP : public ParticleFilter<T> {

	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;

protected:
	std::vector<int> mIndex;																//list of particles to regenerate

	//calculate particle weights - threaded version 
	void CalcWeights(std::vector<Vectorf > &particles);										//calculate particle weights based on model likelihood

	//New particle generation - threaded version 
	void GenerateNewParticles(int k);


};

//Calculate particle weights (mWeights) and find highest likelihood particle. 
//computes an optimal annealing factor and scales the likelihoods. 
template<class T>
void ParticleFilterOMP<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());

	int np = (int)particles.size(), j;
	#pragma omp parallel for																//OpenMP parallelized loop to compute log-likelihoods
	for(j = 0; j < np; j++) 
	{	bool vflag;
		int n = omp_get_thread_num();
		mWeights[j] = mModel->LogLikelihood(particles[j], vflag, n);						//compute log-likelihood weights for each particle
		valid[j] = vflag ? 1 : 0;
	}
	uint 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 log-likelihood
	}
	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 - threaded
template<class T> 
void ParticleFilterOMP<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;
	#pragma omp parallel for
	for(int i = 0; i < mNParticles; i++)													//distribute new particles randomly according to model stdDevs
	{	mNewParticles[i] = mParticles[mIndex[i]];											//add new particle for each entry in each bin distributed randomly about duplicated particle
		AddGaussianNoise(mNewParticles[i], mModel->StdDevs()[k], mRnd[i]);
	}
}


#endif