#ifndef _SFILTER_H
#define _SFILTER_H

#include "stemplate.h"
#include "observer.h"
#include "filter.h"
#include "cv.h"
#include "math.h"
#include "time.h"

using namespace std;

/* This is the worst random number routine in the known universe, but
   I use it for portability. Feel free to replace it. */
double uniform_random(void)
{
  return (double) rand() / (double) RAND_MAX;
}

/* This Gaussian routine is stolen from Numerical Recipes and is their
   copyright. */
double gaussian_random(void)
{
  static int next_gaussian = 0;
  static double saved_gaussian_value;

  double fac, rsq, v1, v2;

  if (next_gaussian == 0) {
    do {
      v1 = 2.0*uniform_random()-1.0;
      v2 = 2.0*uniform_random()-1.0;
      rsq = v1*v1+v2*v2;
    } while (rsq >= 1.0 || rsq == 0.0);
    fac = sqrt(-2.0*log(rsq)/rsq);
    saved_gaussian_value=v1*fac;
    next_gaussian=1;
    return v2*fac;
  } else {
    next_gaussian=0;
    return saved_gaussian_value;
  }
}

class SplineFilter : public Filter<ShapeState>
{
public:
	// FIXME srand()
	SplineFilter( Observer *o ) { ob = o; srand(time(NULL)); }
	/*
	 * something like  exp(-||ob.last - sample||)
	virtual double fixWeight(int i);
	virtual ShapeState moveSample( ShapeState& sample );
	 */

	virtual double SplineFilter::fixWeight( int i )
	{
		double err = sqrt( ob->fitTransform( next[i] ) );
#ifdef DEBUG
		cout << "Sample " << next[i] << " error: " << err << " weight: " << exp(-err) << endl;
#endif
		return exp( -err );
/*
		ShapeState diff = ob->last - next[i];
		diff.coords[2] = 0;
		diff.coords[3] = 0;
#ifdef DEBUG
		cout << "Observed difference: " << ob->last << " vs " << next[i] << endl;
		cout << "  Weight: " << exp(-diff.mag()) << endl;
#endif
		return exp(-diff.mag());
*/
	}

	int findBest()
	{
		if (weight.size() < 1) return -1;
		int max =0;
		for (int i=0; i<weight.size(); ++i)
			if (weight[i] > weight[max])
				max = i;
		return max;
	}

	ShapeState findBestSample()
	{
		int max = findBest();
		if (max == -1) return ShapeState();
		return next[findBest()];
	}

	void displayBest( IplImage* image )
	{
		int max = findBest();
		if (max == -1) return;
#ifdef DEBUG
		cout << "Best sample " << next[max] << endl;
#endif
		cvCircle( image, VEC2POINT(next[max]), 20, cvScalar(0x1133ff) );
	}

	virtual ShapeState moveSample( ShapeState& sample )
	{
		ShapeState res = sample;

		// Model the system as:
		// x_t+1 = x_t + Ax_t + Bu_t
		// where x_t is the state vector having all 8 values 
		// x(:4) are positions, x(4:) are velocities
		// A is the matrix modeling sytem dynamics, u_t is random noise

		// Move the x/y parts first; they get more noise
		res[0] = sample[0] + sample[4] + 0.4*gaussian_random();
		res[1] = sample[1] + sample[5] + 0.4*gaussian_random();

		// Then the rotation / scaling
		res[2] = sample[2] + sample[6] + 0.0001*gaussian_random();
		res[3] = sample[3] + sample[7] + 0.0002*gaussian_random();

		// x velocity and y velocity
		res[4] = sample[4] + 0.8*gaussian_random();
		res[5] = sample[5] + 0.8*gaussian_random();

		// angluar / scaling velocity
		res[6] = sample[6] + 0.00002*gaussian_random();
		res[7] = sample[7] + 0.00002*gaussian_random();
#ifdef DEBUG
		cout << "Selected sample: " << sample << " --> " << res << endl;
#endif
		return res;
	}

protected:
	Observer *ob;
};

#endif