#ifndef _OBSERVER_H
#define _OBSERVER_H

#include "cv.h"
#include "spline.h"
#include "stemplate.h"

class Observer
{
	public:
	Spline *d;
	SplineTemplate st;
	float normalLength;
	ShapeState last;
	bool observing;
	IplImage *lastImage;
	vec3d centroid;
	int Nb;

	// Matricies for the crazyness of projection
	// Q = WX + Q0, where X is our shape vector, W is the mapping, and Q0
	// is the base template shape.
	CvMat *W;  // Mapping from shape space to control points
	CvMat *Q0; // Base template
	CvMat *Qf; // Deformed spline we want to project into shape space
	CvMat *Qt; // Deformed spline we want to project into shape space
	CvMat *Wp; // Pseudo inverse of W
	CvMat *B;  // This is the spline metric matrix
	CvMat *U;  // U = I_2 KRONECKER B

	Observer() {normalLength = 40; d = NULL;}
	void drawNormals(IplImage* image, Spline *sp);
	void draw(IplImage *image);
	vec3d findNearestEdge(IplImage* image, CvPoint pt1, CvPoint pt2);
	void fullyTweakSpline(IplImage* image);
	void fullyTweakSpline(Spline& d);
	void startObserving();
	ShapeState observe(IplImage *image, ShapeState estimate);
	void addPoint(int x, int y);
	double fitTransform( ShapeState& x );
	/*
	 * When points are added I want to reset the observer, hopefully it should be
	 * okay to add something like this
	 */
	void stopObserving()
	{
		observing = false;
		st.s += centroid;
	}
};

#endif // _OBSERVER_H