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

CurveIterator::CurveIterator(Curve *curve, unsigned int steps, CurveIterationMode basicOrLength) {
	this->curve = curve;
	this->steps = steps;
	this->iterationMode = basicOrLength;
	leftBound = curve->getLeftBound();
	rightBound = curve->getRightBound();
	estimatedLength = 0;
	resetIterator();
	if (iterationMode == CurveIterationMode::LENGTH) {
		computeLength();
	}
}

void CurveIterator::resetIterator() {
	basicStepCounter = 0;
	lengthStepCounter = leftBound;
	lastIncrement = (rightBound - leftBound) / steps;
	//std::cout << "lastIncrement after reset is " << lastIncrement << std::endl;
}

void CurveIterator::computeLength() {
	for (int i = 0; i < steps; i++) {
		glm::vec3 startPoint = curve->evaluate(leftBound + (rightBound - leftBound) * i / steps);
		glm::vec3 endPoint = curve->evaluate(leftBound + (rightBound - leftBound) * (i + 1) / steps);

		estimatedLength += glm::length(endPoint - startPoint);
		//std::cout << "segment length " << estimatedLength << std::endl;
	}
}

float CurveIterator::getStep() {
	if (iterationMode == CurveIterationMode::LENGTH) {
		return lengthStepCounter;
	}
	else {
		return leftBound + (rightBound - leftBound) * basicStepCounter / steps;
	}
}

void CurveIterator::nextStep() {
	if (iterationMode == CurveIterationMode::LENGTH) {
		float increment = lastIncrement;
		//std::cout << "lastInc is " << lastIncrement << std::endl;
		//std::cout << "lstepCount is " << lengthStepCounter << std::endl;
		if ((lengthStepCounter + increment) <= rightBound) {
			glm::vec3 point1 = curve->evaluate(lengthStepCounter);
			glm::vec3 point2 = curve->evaluate(lengthStepCounter + increment);
			increment *= ((estimatedLength / steps) / glm::length(point2 - point1));
			//std::cout << "segment length " << glm::length(curve->evaluate(lengthStepCounter + increment) - curve->evaluate(lengthStepCounter)) << std::endl;
			lengthStepCounter += increment;
			lastIncrement = increment;
		}
		else {		//cycle
			resetIterator();
		}
	}
	else {
		if(basicStepCounter < steps){
			basicStepCounter++;
		}
		else {
			resetIterator();
		}
	}
}

void CurveIterator::setProgress(float at) {
	if (at < 0 || at > 1) {
		std::stringstream err;
		err << "CurveIterator : required progress (" << at << ") is out of range [ 0 , 1 ]";
		throw std::invalid_argument(err.str());
	}
	else {
		resetIterator();
		int nSteps = at * steps;
		for (int i = 0; i < nSteps; i++)
			nextStep();
	}
}

glm::vec3 CurveIterator::evaluation() {
	float at;
	if (iterationMode == CurveIterationMode::LENGTH) {
		at = lengthStepCounter;
	}
	else {
		at = leftBound + (rightBound - leftBound) * basicStepCounter / steps;
	}
	//std::cout << "iterator at is " << at << std::endl;
	return curve->evaluate(at);
}

glm::vec3 CurveIterator::derivation() {
	float at;
	if (iterationMode == CurveIterationMode::LENGTH) {
		at = lengthStepCounter;
	}
	else {
		at = leftBound + (rightBound - leftBound) * basicStepCounter / steps;
	}
	return curve->derivate(at);
}

Curve* CurveIterator::getCurve() {
	return curve;
}