#ifndef _FILTER_H
#define _FILTER_H

#include <vector>
#include <iostream>

template <class S>
class Filter
{
	public:
	void updateSamples();
	void samples( std::vector<S>& data );
	// replicate one sample n times, for initialization
	void nsamples( S& data, int n );

	protected:
	std::vector<S> cur,next;
	std::vector<double> cdf,weight;
	void predict();
	void makeCdf();
	virtual double fixWeight(int i) { return 1.0; }
	virtual S moveSample( S& sample ) { return sample; }

};

/*
 * Nonsense due to linking problems with templated classes
 */

using namespace std;

template <class S>
void Filter<S>::nsamples( S& data, int n )
{
	next.resize(n);
	weight.resize(next.size());
	cdf.resize(next.size());
	for (int i = 0; i < next.size(); ++i)
	{
		next[i] = data;
		weight[i] = 1;
		cdf[i] = i+1; //cdf[n] holds the value of sum[-infty,n] weight
	}
}

template <class S>
void Filter<S>::samples( vector<S>& data  )
{
	if (&next != &data)
		next = data;
	weight.resize(next.size());
	cdf.resize(next.size());
	for (int i = 0; i < next.size(); ++i)
	{
		weight[i] = 1;
		cdf[i] = i+1; //cdf[n] holds the value of sum[-infty,n] weight
	}
}

template <class S>
void Filter<S>::predict()
{
	next.resize(cur.size());
	for (int i=0; i<cur.size(); ++i)
	{
		// pick a random number then find it's nearest match(>=) in the cdf
		// TODO check that this works with the cdf
		double choice = (cdf[cdf.size()-1]*rand()/(RAND_MAX+1.0) );
		int low, middle, high;

		low = 0;
		high = cdf.size();

		while (high>(low+1)) {
			middle = (high+low)/2;
			if (choice < cdf[middle])
				high = middle;
			else 
				low = middle;
		}
#ifdef DEBUG
		cout << "Sample advances: " << cur[high] << endl;
#endif
    
		next[i] = moveSample(cur[high]);
	}
}


template <class S>
void Filter<S>::makeCdf()
{
	if (cdf.size() < 1) return;

	for (int i=0; i<weight.size(); ++i)
	{
		weight[i] = fixWeight(i);
		cdf[i] = ( i==0 ? 0 : cdf[i-1] ) + weight[i];
	}

	if ( cdf[cdf.size()-1] == 0 )
		for (int i = 0; i < next.size(); ++i)
		{
			weight[i] = 1;
			cdf[i] = i+1; //cdf[n] holds the value of sum[-infty,n] weight
		}
	else
		for (int i=0; i<cdf.size(); ++i)
		{
			cdf[i] /= cdf[cdf.size()-1];
#ifdef DEBUG
			cout << "\t" << cdf[i] << endl;
#endif
		}
}

template <class S>
void Filter<S>::updateSamples()
{
	cur = next;

	/*
	 * pick some of the old samples and map them to new samples using a model
	 * picking is weighted
	 */
	predict();
  
	/*
	 * determine the importance of each new sample based on data from observations
	 * recreate the cumulative distribution
	 */
	makeCdf();
}

#endif