#include "spline.h"

//------------------------------------------------------------------------------
// Creates a vector that has the coefficients of p_i-1 p_i p_i+1 p_i+2 in it,
// for Catmull-Rom interpolation. t is the normalized time.
vec<double,4> Spline::getCatMull(double t, double k)
{

	vec<double,4> ret;
	vec<double,4> T;
	T[0] = 1;
	T[1] = t;
	T[2] = t*t;
	T[3] = t*t*t;

	vec<double,4> K[4];
	// First index is the column of the K matrix.
	K[0][0] = 0;
	K[0][1] = -k;
	K[0][2] = 2*k;
	K[0][3] = -k;

	K[1][0] = 1;
	K[1][1] = 0;
	K[1][2] = k-3;
	K[1][3] = 2-k;

	K[2][0] = 0;
	K[2][1] = k;
	K[2][2] = 3-2*k;
	K[2][3] = k-2;

	K[3][0] = 0;
	K[3][1] = 0;
	K[3][2] = -k;
	K[3][3] = k;

	for (int i=0; i<4;i++) {
		ret[i] = dot(T,K[i]);
	}
	return ret;
}

void Spline::setPoint(vec3d point)
{
	// New point
	points.push_back(point);
	/*
	for ( vector<vec3d>::iterator I = points.begin(); I != points.end(); ++I)
		cout << *I;
	cout << endl;
	*/
	//cout << point << endl;
}

int Spline::findState(float time)
{
	// Since 'time' doesn't exist, just take the floor
	if (points.size()) {
		if (time < points.size())
			return (int) floor(time);
//		cout << "CRAP" << endl;
		return points.size();
	}

	// Error
	return -1;
}

//------------------------------------------------------------------------------
// Catmull-Rom spline interpolation
int Spline::getSplinePoint(float time, vec3d &vnew)
{
	if (points.size() < 2) {
		return -1;
	}


	int f = findState(time);
	if (f == -1)
		return -1;

	if (f == points.size()) {
//		cout << "GRRRR" << endl;
		vnew = points[points.size()-1];
		return points.size()-1;
	}

	// The concept of time is pointless for space curves; so time is 0-1
	// between each frame, so total time is 0..N-1 for N frames.
	float normaltime = time - f;

	vec<double,4> catMul = getCatMull(normaltime, 0.5);

	// The special cases, f==0 and f==N-1 require assigning
	// P_i+2 = P_i+1 and P_i-1 = P_i. (This are the endpoints)
	vec3d v1 = points[f];

	vec3d v0 = v1;
	if (f > 0)
		v0 = points[f-1];

	vec3d v2 = v1;
	if (f < points.size()-1)
		v2 = points[f+1];

	vec3d v3 = v2;
	if (f < (int) points.size()-2)
		v3 = points[f+2];

	for (int j=0; j < 2; j++) {
		vec<double,4> temp;
		temp[0] = v0[j];
		temp[1] = v1[j];
		temp[2] = v2[j];
		temp[3] = v3[j];
		vnew[j] = dot(temp, catMul);
	}
	return f;
}

#define EPS 0.01

// I'm sure there's a smarter way to do this but whatever
vec3d Spline::getNormal(float time)
{
	vec3d p1;
	vec3d p2;
	getSplinePoint(time-EPS, p1);
	getSplinePoint(time+EPS, p2);
	vec3d n = p2-p1;
	n /= n.mag();
	n[2] = n[0];
	n[0] = -n[1];
	n[1] = n[2];
	n[2] = 0;

	return n;
}

// Given a point index, calculate the normal exactly
vec3d Spline::getNormalPoint(int pt)
{
	if (pt >= points.size() || points.size() < 2) 
		return vec3d();

	vec3d n;
	if (pt == 0) {
		n = points[1]-points[0];
	} else if (pt == points.size()-1) {
		n = points[pt] - points[pt-1];
	} else {
		n = points[pt+1] - points[pt-1];
	}

	n /= n.mag();
	n[2] = n[0];
	n[0] = -n[1];
	n[1] = n[2];
	n[2] = 0;

	return n;
}

// Divides the spline up into line sections and draws it
void Spline::drawSpline(IplImage* image)
{
	vec3d p1;
	vec3d p2;

	if (points.size() < 4) 
		return;

	int steps = 50;
	float dt = points.size() / ((float) steps);

	float t = 0;
	getSplinePoint(t, p1);
	for(int i=0; i<steps;i++) {
		t = i*dt;
		getSplinePoint(t+dt, p2);

		cvLine( image,  VEC2POINT(p1),
				VEC2POINT(p2), cvScalar(150.,250.), 2);
		p1 = p2;
	}
}

vec3d Spline::findCentroid()
{
	// Makes flagrant assumption that centroid of control points ==
	// centroid of curve
	vec3d c;
	for(int i = 0;i<points.size();i++) {
		c += points[i];
	}
	double t = 1.0/(double)points.size();
	c = t*c;
	return c;
}

void operator+= (Spline &sp, vec3d v)
{
	for (int i = 0; i<sp.points.size(); i++) {
		sp.points[i] += v;
	}
}