#include "interpolators.h"

rInterpolator::rInterpolator()
{
	startFrame = endFrame = -1;
}

void rInterpolator::attachVar(vector3 *xi)
{
	x = xi;
}

rBezier::rBezier()
{
	finished = false;
	curPoint = -1;
	sfA = 0;
	xA = 0;
}

rBezier::~rBezier()
{
	delete [] xA;
}

void rBezier::setStartPoint(float startframe)
{
	startFrame = startframe;
}

void rBezier::CalcMidPoints(vector3 *p)
{
	
	int i = 2;
	int c = 2;
	vector3 temp;
	vector3 temp2;
	float mag;
	xA = new vector3 [ 3*size - 2];
	xA[0] = xA[1] = p[0];
	while(c<size)	//This loop adds the intermediate values for the Bezier path
	{
		temp = p[c] - p[c-2];
		mag = m->MagnitudeVA3((float*)&temp);
		if(mag>1)temp = temp/mag;

		temp2 = p[c-1] - p[c-2];
		mag = m->MagnitudeVA3((float*)&temp2);
		temp2 = p[c] - p[c-1];
		mag += m->MagnitudeVA3((float*)&temp2);
		if(mag == 0)
			mag = 1.0f;

		mag *= 0.5f;

		xA[i+1] = p[c-1];
		xA[i] = xA[i+1] - temp*(0.35f*mag);

		temp2 = p[c] - p[c-1];

		xA[i+2] = xA[i+1] + temp*(0.35f*mag);
		i = i + 3;
		c++;
	}
	xA[i] = p[size-1];
	xA[i+1] = xA[i];
}


void rBezier::attachPath(vector3 *xArray, float *sfArray, int Size)
{
	if(Size<3)
		return;	//We need at least 3 points
	size = Size;
	CalcMidPoints(xArray);
	sfA = new float [size];
	m->MemCopy(sfA, sfArray, size*sizeof(float));

}
bool rBezier::Update()
{
	if(xA == 0)
		return false;
	if(float(curFrame) < endFrame)
	{
		t = (float(curFrame) - startFrame)/sfA[curPoint];
		vector3 lx = a*(t*t*t) + b*(t*t) + c*t + xA[3*curPoint];
		*x = lx;
	}else if(curPoint>size-3)
	{
		finished = true;
	}else
	{
		curPoint++;
		int loc = 3*curPoint;

		c = (xA[loc+1] - xA[loc])*3;
		b = (xA[loc+2] - xA[loc+1])*3 - c;
		a = xA[loc+3] - xA[loc] - c - b;

		startFrame = (float)curFrame;
		endFrame = startFrame + sfA[curPoint];
	} 

	return !finished;
}
