/*
 * (C) C3R14L.K1L4 / João Carlos Ferreira Gonçalves
 * further contact: jcgonc@student.dei.uc.pt
 * started on 09-04-2007
 * checked on 14-04-2007: OK, clean
 */
#ifndef C3R14L_VECTOR3FSPLINEINTERPOLATOR_HEADER
#define C3R14L_VECTOR3FSPLINEINTERPOLATOR_HEADER

#include "CatmullRomSpline.h"
#include "Vector3f.h"

class Vector3fSplineInterpolator {
public:
	Vector3f* position;
	Vector3f* *path;
	int arraylength;
	int pathsize;
	int ncurrentaddingpoint;

public:
	Vector3fSplineInterpolator(Vector3f* *_path, int _arraylength, int _pathsize) {
		position = new Vector3f();
		path = _path;
		arraylength = _arraylength;
		ncurrentaddingpoint = _pathsize;
		pathsize = _pathsize;
	}

	Vector3fSplineInterpolator(int numberofpathpoints) {
		ncurrentaddingpoint = 0;
		pathsize = 0;
		arraylength = numberofpathpoints;
		position = new Vector3f();
		path = (Vector3f**) malloc(sizeof(Vector3f*) * numberofpathpoints);
	}

	~Vector3fSplineInterpolator() {
		delete (position);
		delete (path);
	}

	void addPathPoint(Vector3f* newposition) {
		//add data
		path[ncurrentaddingpoint] = new Vector3f(newposition);
		ncurrentaddingpoint++;
		pathsize++;
	}

	void resetAddingPoint() {
		ncurrentaddingpoint = 0;
	}

	//***

	void interpolateVector(int startingpoint, float n) {
		// control points, s1 & s2 are the interpolated ones
		int s0, s1, s2, s3;
		// cycle path loop
		if (startingpoint >= pathsize) {
			startingpoint = startingpoint % pathsize;
		}
		// check for loop positions, each one has it's own control points
		if (startingpoint == 0) {
			s0 = pathsize - 1;
			s1 = 0;
			s2 = 1;
			s3 = 2;
		} else if (startingpoint == pathsize - 2) {
			s0 = pathsize - 3;
			s1 = pathsize - 2;
			s2 = pathsize - 1;
			s3 = 0;
		} else if (startingpoint == pathsize - 1) {
			s0 = pathsize - 2;
			s1 = pathsize - 1;
			s2 = 0;
			s3 = 1;
		} else {
			s0 = startingpoint - 1;
			s1 = startingpoint;
			s2 = startingpoint + 1;
			s3 = startingpoint + 2;
		}
		float x1 = path[s0]->x;
		float x2 = path[s1]->x;
		float x3 = path[s2]->x;
		float x4 = path[s3]->x;

		float y1 = path[s0]->y;
		float y2 = path[s1]->y;
		float y3 = path[s2]->y;
		float y4 = path[s3]->y;

		float z1 = path[s0]->z;
		float z2 = path[s1]->z;
		float z3 = path[s2]->z;
		float z4 = path[s3]->z;

		position->x = catmullRomSpline(x1, x2, x3, x4, n);
		position->y = catmullRomSpline(y1, y2, y3, y4, n);
		position->z = catmullRomSpline(z1, z2, z3, z4, n);
	}

	void interpolateVector(float n) {
		int pos = floor(n);
		float coef = n - float(pos);
		interpolateVector(pos, coef);
	}

	int getNumberOfPathPoints() {
		return pathsize;
	}

	Vector3f* getCurrentPosition() {
		return position;
	}
};
#endif
