#include "Bezier.hpp"
#include <iostream>

#define LERP(t,a,b) (1 - t) * a + t * b;

Bezier::Bezier(unsigned int order, vector<glm::vec3> *points, vector<float> *intervals) : Curve{ points, intervals } {
	this->order = order;
}

/*might be useless (using lerp based evaluation)*/
float Bezier::evaluateBasisFunction(float at, int number, int order, float intervalLeft, float intervalRight) {
	if (number < 0 || number > order) {
		return 0;
	}
	else if (number == 0 && order == 0) {
		return 1;
	}
	else {
		return ((at - intervalLeft)/(intervalRight - intervalLeft)) * evaluateBasisFunction(at, number - 1, order - 1, intervalLeft, intervalRight) + ((intervalRight - at) / (intervalRight - intervalLeft)) * evaluateBasisFunction(at, number, order - 1, intervalLeft, intervalRight);
	}
}

glm::vec3 Bezier::evaluate(float at) {
	vector<glm::vec3>partials = vector<glm::vec3>();
	for (const glm::vec3 point : *controlPoints)
		partials.push_back(point);
	int iter = partials.size() - 1;
	while (iter > 0) {
		for (int i = 0; i < iter; i++) {
			partials[i] = LERP(at, partials[i], partials[i + 1]);
		}
		iter--;
	}
	return partials[0];
}

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

/*
* takes as input a sequence of points 
* to be interpreted as conjoined bezier segments of order 3
*/
Bezier3Segments::Bezier3Segments( vector<glm::vec3>* points, vector<float>* intervals) : Bezier { ((unsigned int) points->size() - 1 ) , points , intervals } {
	int nPoints = points->size();
	assert((nPoints % 4) == 0);
}

glm::vec3 Bezier3Segments::evaluate(float at) {
	for (int interval = 0; interval < intervalBoundaries->size() - 1; interval++) {
		if ((intervalBoundaries->operator[](interval) <= at) && (intervalBoundaries->operator[](interval + 1) >= at)) {
			float normalizedAt = (at - intervalBoundaries->operator[](interval)) / (intervalBoundaries->operator[](interval + 1) - intervalBoundaries->operator[](interval));
			vector<glm::vec3> partials;
			for (int j = 0; j <= 3; j++) {
				partials.push_back(controlPoints->operator[](4 * interval + j));
			}
			int count = 3;
			while (count > 0) {
				for (int j = 0; j < count; j++) {
					partials[j] = LERP(normalizedAt, partials[j], partials[j + 1]);
				}
				count--;
			}
			return partials[0];
		}
	}
	throw "Bezier3Segment evaluation out of range";
}

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

float Bezier3Segments::getLeftBound() {
	return intervalBoundaries->front();
}

float Bezier3Segments::getRightBound() {
	return intervalBoundaries->back();
}