#include "stemplate.h"

#define VEC2POINT(x) cvPoint( (int)round((x)[0]), (int)round((x)[1]) )
using namespace std;

void SplineTemplate::addPoint(int x, int y)
{
	s.setPoint(vec3d((double)x,(double)y,0));
}

void SplineTemplate::setTemplate()
{
	// Center at the origin
	s += -s.findCentroid();

	d.points.clear();
	// Make derived same dimension
	for (int i=0;i<s.points.size();i++) {
		d.setPoint(s.points[i]);
	}
}

void SplineTemplate::setDerived(ShapeState state)
{
	// Q = WX + Q0
	for (int i=0;i<s.points.size();i++) {
		d.points[i][0] = state[0]
			+ s.points[i][0]*state[2]
			- s.points[i][1]*state[3]
			+ s.points[i][0];
		d.points[i][1] = state[1]
			+ s.points[i][1]*state[2]
			+ s.points[i][0]*state[3]
			+ s.points[i][1];
	}
}

double SplineTemplate::mse(Spline& other)
{
	if (other.points.size() != d.points.size())
		return 1e9;

	double mse =0;
	for (int i=0; i<other.points.size(); ++i)
		if (other.points[i][2] == 0)
			mse += (other.points[i] - d.points[i]).magsq();
		else
			mse += 500;
	return mse;
}