#include "../../HEADERS/CURVES/NURBS.hpp"
#include <stdexcept>
#include <sstream>
#include <iostream>

void NURBS::preliminaryChecks() {
	int nPoints = controlPoints->size();
	int nWeights = weights->size();
	int nBoundaries = intervalBoundaries->size();
	int nMultiplicities = multiplicities->size();
	int requiredBoundaries = order + 1;
	for (int i = 0; i < nMultiplicities; i++)
		requiredBoundaries += multiplicities->operator[](i);

	std::cout << "nPoints : " << nPoints << std::endl;
	std::cout << "nWeights : " << nWeights << std::endl;
	std::cout << "nBoundaries : " << nBoundaries << std::endl;
	std::cout << "nMultiplicities : " << nMultiplicities << std::endl;
	std::cout << "requiredBoundaries : " << requiredBoundaries << std::endl;

	if (nWeights != nPoints)
		throw std::invalid_argument("NURBS : nPoints <> nWeights");
	if (nMultiplicities != (nPoints)) {
		std::stringstream err;
		err << "NURBS : nPoints (" << nPoints << ") <> nMultiplicities (" << nMultiplicities << ")";
		throw std::invalid_argument(err.str());
	}

	if (nBoundaries != requiredBoundaries) {
		std::stringstream err;
		err << "NURBS : nBoundaries (" << nBoundaries << ") <> should be equal to Order + sum (multiplicities) (" << requiredBoundaries << ")";
		throw std::invalid_argument(err.str());
	}
}

NURBS::NURBS( unsigned int order, vector<glm::vec3>* points, vector<float>* weights, vector<float>* boundaries, vector<unsigned int>* multiplicities) : Curve{ points , boundaries} {
	this->order = order;
	this->weights = weights;
	this->intervalBoundaries = boundaries;
	this->multiplicities = multiplicities;
	preliminaryChecks();
	derivativePoints = new vector<glm::vec3>();
	derivativeWeghts = new vector<float>();
	derivativeBoundaries = new vector<float>();

	for (int i = 0; i < controlPoints->size() - 1; i++) {

		glm::vec3 point = ((float)order) * ((*controlPoints)[i + 1] - (*controlPoints)[i])
			/ ((*intervalBoundaries)[i + order + 1] - (*intervalBoundaries)[i + 1]);
		derivativePoints->push_back(point);
		//not so sure about this
		derivativeWeghts->push_back(weights->at(i));

	}
	
	for (int i = 1; i < intervalBoundaries->size() - 1; i++)
		derivativeBoundaries->push_back(intervalBoundaries->operator[](i));
}

glm::vec3 NURBS::deBoor(float at, int index) {
	if (order < 0) {
		throw std::invalid_argument("NURBS : evaluateBasis order underflow");
	}
	else {
		vector<glm::vec3> points = vector<glm::vec3>();
		vector<float> wei = vector<float>();					//
		int firstPointIndex = index - order;
		for (int i = index - order; i <=  index; i++) {
			float w = weights->operator[](i);
			points.push_back(controlPoints->operator[](i) * w);
			wei.push_back(w);				//
		}
		for (int j = 0; j < order; j++) {
			for (int i = index - order + j + 1; i <= index; i++) {
				int pointIndex = i - index + order;
				float denominator = ((*intervalBoundaries)[i + order - j] - (*intervalBoundaries)[i]);
				if (denominator != 0.0f){
						points[pointIndex] = (points[pointIndex] * (at - (*intervalBoundaries)[i]) + ((*intervalBoundaries)[i + order - j] - at) * points[pointIndex - 1])
						/ denominator;
						wei[pointIndex] = (wei[pointIndex] * (at - (*intervalBoundaries)[i]) + ((*intervalBoundaries)[i + order - j] - at) * wei[pointIndex - 1])
						/ denominator;
				}
			}
		}
		return points[order] / wei[order];
	}
	
}

glm::vec3 NURBS::evaluate(float at) {
	//std::cout << "eval at " << at << std::endl;
	for(int i = order; i < intervalBoundaries->size() - order - 1; i++)
		if ((intervalBoundaries->operator[](i) <= at) && (at < intervalBoundaries->operator[](i + 1))) {
				return deBoor(at, i);
		}
	if (pow(at - getRightBound(), 2) < 1e-8)
		return deBoor(at, intervalBoundaries->size() - order - 2);
	
	throw std::invalid_argument("NURBS : evaluation out of range");

}

glm::vec3 NURBS::derivate(float at) {
	vector<glm::vec3>* swapPoints = controlPoints;
	vector<float>* swapWeights = weights;
	vector<float>* swapBoundaries = intervalBoundaries;

	controlPoints = derivativePoints;
	weights = derivativeWeghts;
	intervalBoundaries = derivativeBoundaries;

	order--;
	glm::vec3 result = evaluate(at);
	order++;

	controlPoints = swapPoints;
	weights = swapWeights;
	intervalBoundaries = swapBoundaries;
	return result;
}

float NURBS::getLeftBound() {
	return intervalBoundaries->at(order);
}

float NURBS::getRightBound() {
	return intervalBoundaries->at(intervalBoundaries->size() - order - 1);
}