#include "bspline.h"
#include <iostream>
#include <vector>
#include <cassert>
#include <Rcpp.h>

// Find the knot span of the parametric point t.
//
// INPUT:
//   p - spline degree
//   t - parametric point
//   U - knot sequence
//
// RETURN:
//
//   ret - knot span
//
// Note: This is NOT
// Algorithm A2.1 from 'The NURBS BOOK' pg68
// as that algorithm only works for nonperiodic
// knot vectors, nonetheless the results should
// be EXACTLY the same if U is nonperiodic

unsigned int
bspline::findspan
(int p, double t, const std::vector<double>& U)
// ret is the index of the last knot at the left of the point t
{
unsigned int n = U.size();
unsigned int ret = 0;
if (t > U[U.size () - 1] || t < U)
{
Rcpp::Rcerr << "Value " << t
<< " of t is outside the knot span by "
<< U[U.size () - 1] - t << "\n";
Rcpp::stop("Error in the C++ execution");
// exit(EXIT_FAILURE);
}
else
{
while ((ret < n) && (U[ret] <= t))
ret++;
}
if(ret>n-p-2) return n-p-2;
return ret-1;
};	//findspan

void
bspline::basisfun
(unsigned int i, double t, int p, const std::vector<double>& U, Eigen::ArrayXd& N)
/*
Evaluates basis splines which include x=t in the span and save values in N
*/
{

double saved, temp;

// work space
/* BERNHARD:
* the following two lines results in (only in Linux, gcc)
* Found the following significant warnings:
bspline.cpp:62:9: warning: ISO C++ forbids variable length array ‘left’ [-Wvla]
bspline.cpp:63:9: warning: ISO C++ forbids variable length array ‘right’ [-Wvla]
*/
/*double left[p+1];
double right[p+1];*/

double *left = (double*) calloc(p + 1, sizeof(double));
double *right = (double*) calloc(p + 1, sizeof(double));

/* statt dessen probiert:
* (um dynamisch left und right zu erstellen?)
* Alles Errors

double *left = new double[p+1];
double *right = new double[p+1];
left = Rcpp::IntegerVector(p+1);
right = Rcpp::IntegerVector(p+1);

Rcpp::NumericVector left(p+1);
Rcpp::NumericVector right(p+1);
*/

if(i == p && t == U[i]){
N = 1.0;
}
else if(i == U.size() ){
N[U.size()-p-2]= 1.0;
}
else{
std::vector<double> P(p + 1,1.0);
for (unsigned int j = 1; j <= p; j++) {
left[j] = t - U[i + 1 - j];
right[j] = U[i + j] - t;
saved = 0.0;
for (unsigned int r = 0; r < j; r++) {
temp = P[r] / (right[r + 1] + left[j - r]);
P[r] = saved + right[r + 1] * temp;
saved = left[j - r] * temp;
}
P[j] = saved;
}
for (unsigned int k = 0; k <= p; k++) {
N[i - p + k] = P[k];
}
}

free(left);
free(right);

};	//basisfun