#include "Hermite.hpp"

#define PHI0(t) (2.0f*t*t*t - 3.0f*t*t + 1)
#define PHI1(t) (t*t*t - 2.0f*t*t + t)
#define PSI0(t) (-2.0f*t*t*t + 3.0f*t*t)
#define PSI1(t) (t*t*t - t*t)

Hermite::Hermite(vector<glm::vec3> *points, vector<float> *intervalBoundaries)
	: Curve{ points, intervalBoundaries } {
}

void Hermite::computeDerivatives(HermiteModes mode, vector<glm::vec3> auxData) {
	derivatives.clear();
	int length = (*controlPoints).size();
	switch (mode) {
	case HermiteModes::Direct:
		assert(controlPoints->size() == auxData.size());
		derivatives.push_back(auxData[0]);
		for (int i = 0; i < auxData.size() - 1; i++) {
			derivatives.push_back(auxData[i]);
			derivatives.push_back(auxData[i]);
		}
		derivatives.push_back(auxData[auxData.size() - 1]);
		break;
	case HermiteModes::Basic:
	{
		glm::vec3 result;
		result = 0.5f * ((*controlPoints)[1] - (*controlPoints)[0]) / ((*intervalBoundaries)[1] - (*intervalBoundaries)[0]);
		derivatives.push_back(result);
		for (int i = 1; i < length - 1; i++) {
			result = 0.5f *
				(
					(((*controlPoints)[i + 1] - (*controlPoints)[i])
						/ ((*intervalBoundaries)[i + 1] - (*intervalBoundaries)[i]))
					+ (((*controlPoints)[i] - (*controlPoints)[i - 1])
						/ ((*intervalBoundaries)[i] - (*intervalBoundaries)[i - 1]))
					);
			derivatives.push_back(result);
			derivatives.push_back(result);
		}
		result = 0.5f * ((*controlPoints)[length - 1] - (*controlPoints)[length - 2]) / ((*intervalBoundaries)[length - 1] - (*intervalBoundaries)[length - 2]);
		derivatives.push_back(result);
	}
	break;
	case HermiteModes::Cardinal:
	{
		assert(controlPoints->size() == auxData.size());
		int last = controlPoints->size() - 1;
		glm::vec3 delta1;
		glm::vec3 delta2;
		delta1 = (*controlPoints)[1] - (*controlPoints)[0];
		float interval1;
		float interval2;
		interval1 = (*intervalBoundaries)[1] - (*intervalBoundaries)[0];
		glm::vec3 result;
		result = 0.5f * (1 - auxData[0].x) * delta1 / interval1;
		derivatives.push_back(result);
		for (int i = 1; i < last; i++) {
			delta1 = (*controlPoints)[i + 1] - (*controlPoints)[i];
			delta2 = (*controlPoints)[i] - (*controlPoints)[i - 1];
			interval1 = (*intervalBoundaries)[i + 1] - (*intervalBoundaries)[i];
			interval2 = (*intervalBoundaries)[i] - (*intervalBoundaries)[i - 1];
			result = 0.5f * (1 - auxData[i].x) * (delta1 / interval1 + delta2 / interval2);
			derivatives.push_back(result);
			derivatives.push_back(result);
		}
		delta1 = (*controlPoints)[last] - (*controlPoints)[last - 1];
		interval1 = (*intervalBoundaries)[last] - (*intervalBoundaries)[last - 1];
		result = 0.5f * (1 - auxData[last].x) * (delta1 / interval1);
		derivatives.push_back(result);
	}
	break;
	case HermiteModes::TBC:
	{
		assert(controlPoints->size() == auxData.size());
		int last = controlPoints->size() - 1;
		glm::vec3 delta1;
		glm::vec3 delta2;
		delta1 = (*controlPoints)[1] - (*controlPoints)[0];
		float interval1;
		float interval2;
		interval1 = (*intervalBoundaries)[1] - (*intervalBoundaries)[0];
		glm::vec3 result;
		//first
		result = 0.5f * (1 - auxData[0].x) * (1 + auxData[0].y) * (1 + auxData[0].z) * delta1 / interval1;
		derivatives.push_back(result);
		for (int i = 1; i < last; i++) {
			delta1 = (*controlPoints)[i + 1] - (*controlPoints)[i];
			delta2 = (*controlPoints)[i] - (*controlPoints)[i - 1];
			interval1 = (*intervalBoundaries)[i + 1] - (*intervalBoundaries)[i];
			interval2 = (*intervalBoundaries)[i] - (*intervalBoundaries)[i - 1];
			//last
			result = 0.5f * ((1 - auxData[i].x) * (1 + auxData[i].y) * (1 - auxData[i].z) * (delta1 / interval1) + (1 - auxData[i].x) * (1 - auxData[i].y) * (1 - auxData[i].z) * (delta2 / interval2));
			derivatives.push_back(result);
			//first
			result = 0.5f * ((1 - auxData[i].x) * (1 + auxData[i].y) * (1 + auxData[i].z) * (delta1 / interval1) + (1 - auxData[i].x) * (1 - auxData[i].y) * (1 - auxData[i].z) * (delta2 / interval2));
			derivatives.push_back(result);
		}
		delta1 = (*controlPoints)[last] - (*controlPoints)[last - 1];
		interval1 = (*intervalBoundaries)[last] - (*intervalBoundaries)[last - 1];
		//last
		result = 0.5f * ((1 - auxData[last].x) * (1 + auxData[last].y) * (1 - auxData[last].z) * (delta1 / interval1));
		derivatives.push_back(result);
	}
	break;
	}
}

glm::vec3 Hermite::evaluateCubic(float t, float t1, float t2, glm::vec3 y1, glm::vec3 y2, glm::vec3 dy1, glm::vec3 dy2) {
	float interval = t2 - t1;
	float newX = (t - t1) / interval;
	return y1 * PHI0(newX) + dy1 * interval * PHI1(newX) + y2 * PSI0(newX) + dy2 * interval * PSI1(newX);
}

glm::vec3 Hermite::evaluate(float at) {
	for (int i = 0; i < intervalBoundaries->size() - 1; i++)
		if ((*intervalBoundaries)[i] <= at && at <= (*intervalBoundaries)[i + 1])
			return evaluateCubic(at, (*intervalBoundaries)[i], (*intervalBoundaries)[i + 1], (*controlPoints)[i], (*controlPoints)[i + 1], derivatives[2 * i], derivatives[2 * i + 1]);
	return glm::vec3();
}

glm::vec3 Hermite::derivate(float at) {
	return glm::vec3();
}