#include <stdio.h>
#include <NxPhysics.h>
#include "../headers/MInterpolate.h"
#include "../headers/MVector3.h"

template <typename vec>
MInterpolate<vec>::MInterpolate(vec* vector, vec target, int interpolationType) {
	interpolatedScalar = new MInterpolatedScalar(interpolationType);
	this->current = vector;
	setTarget(target);
}

template <typename vec>
MInterpolate<vec>::MInterpolate(vec* vector, int interpolationType) {
	interpolatedScalar = new MInterpolatedScalar(interpolationType);
	this->current = vector;
	setTarget(*vector);
}

template <typename vec>
MInterpolate<vec>::~MInterpolate(void) {
	delete interpolatedScalar;
}

template <typename vec>
void MInterpolate<vec>::setTarget(vec target) {
	this->target = target;
//	this->target.normalize();
	start = *current;
	interpolatedScalar->reset();
	diffVector = target - *current;
}


template <typename vec>
vec& MInterpolate<vec>::getTarget() {
	return target;
}

template <typename vec>
void MInterpolate<vec>::update(void) {
	if(interpolatedScalar->hasNext()) {
		float nextFloat = interpolatedScalar->next();		
		*current = start+diffVector*nextFloat;
	}
}

void MInterpolate<NxQuat>::update(void) {
	if(interpolatedScalar->hasNext()) {
		float nextFloat = interpolatedScalar->next();
		current->slerp(nextFloat, start, target);
	}
}

template class MInterpolate<MVector3>;
template class MInterpolate<NxQuat>;
template class MInterpolate<float>;