/*
 * class Spline - implementation
 *
 * $Author: claudio $
 *
 * $Revision: 1.2 $
 *
 * $Log: spline.cpp,v $
 * Revision 1.2  2019-12-06 13:03:02  claudio
 * gcc 7.4.0
 *
 * Revision 1.1.1.1  2012-09-05 08:17:22  claudio
 * Interpolator utility classes
 *
 */

#include "spline.h"
#include <cmath>
#include <cstdlib>
#include <limits>

namespace Interpolator{

Spline::Spline (doubleVector& xvalues, doubleVector&  yvalues, double yp0 , double ypN ):Interpolator(){
	if(xvalues.size() != yvalues.size()) throw std::length_error("Spline(): data vectors must be same length");
	if(xvalues.size() < 2) throw std::length_error("Spline(): data vector length must be >=2");
	n_base=xvalues.size();
	x_base= static_cast<double*>(malloc(n_base*sizeof(double))); //get one element more the needed - just for testing bug
	y_base= static_cast<double*>(malloc(n_base*sizeof(double)));
	y2 = static_cast<double*>(malloc((n_base+1)*sizeof(double)));
	u = static_cast<double*>(malloc((n_base-1)*sizeof(double)));
	
	//copy the base values into the just allocated arrays
	//and check that xvalues are sorted correctly
	double lowbound=-HUGE_VAL;
	for(unsigned int i=0;i < n_base;i++){
		if(xvalues[i]>lowbound){
			lowbound=xvalues[i];
		}
		else{
			free(u);
			free(y2);
			free(y_base);
			free(x_base);
			throw std::domain_error("Spline(): x_values must be in ascending order");
		}
 
		x_base[i]=xvalues[i];
		y_base[i]=yvalues[i];
	}
	//record range limits
	_min=x_base[0];
	_max=x_base[n_base-1];
	// do the initial calulations for the spline algorithm
	// got from an old Pascal based text about Numerical Algorithms
	{
		double p,sig,qn,un;
		unsigned int i;
		int k;
		//set boundary conditions for intial point
		if(std::isnan(yp0)){
			u[0]=0.0; y2[0]=0.0;//natural spline condition
			}
		else{
			u[0]=(3.0/(x_base[1]-x_base[0]))*((y_base[1]-y_base[0])/(x_base[1]-x_base[0])-yp0);
			y2[0]=-0.5;
		}
		
		for (i=1;i<n_base-1;i++) {
			sig=(x_base[i]-x_base[i-1])/(x_base[i+1]-x_base[i-1]);
			p=sig*y2[i-1]+2.0;
			y2[i]=(sig-1.0)/p;
			u[i]=(y_base[i+1]-y_base[i])/(x_base[i+1]-x_base[i]) - (y_base[i]-y_base[i-1])/(x_base[i]-x_base[i-1]);
			u[i]=(6.0*u[i]/(x_base[i+1]-x_base[i-1])-sig*u[i-1])/p;
		}
		//set boundary conditions for intial point
		if(std::isnan(ypN)){
			qn=un=0.0; //natural spline condition
		}
		else{
			un=(3.0/(x_base[n_base-1]-x_base[n_base-2]))* (ypN-(y_base[n_base-1]-y_base[n_base-2])/(x_base[n_base-1]-x_base[n_base-2]));
			qn=0.5;
		}
		y2[n_base-1]=(un-qn*u[n_base-2])/(qn*y2[n_base-2]+1.0);
		for (k=n_base-2;k>=0;k--)
			y2[k]=y2[k]*y2[k+1]+u[k];
	}
}

Spline::Spline(Spline& src):Interpolator()
{
	if( src.n_base < 2 ) throw std::length_error("Spline(): data vector length must be >=2");
	n_base=src.n_base;
	x_base= static_cast<double*>(malloc(n_base*sizeof(double)));
	y_base= static_cast<double*>(malloc(n_base*sizeof(double)));
	y2 = static_cast<double*>(malloc((n_base+1)*sizeof(double)));
	u = static_cast<double*>(malloc((n_base-1)*sizeof(double)));
	_min=src._min;
	_max=src._max;
	for(unsigned int i=0; i<n_base;i++){
		x_base[i]=src.x_base[i];
		y_base[i]=src.y_base[i];
		y2[i]=src.y2[i];
	}
	y2[n_base]=src.y2[n_base];
	for(unsigned int i=0; i<n_base-1;i++){
		u[i]=src.u[i];
	}
}

Spline& Spline::operator=(const Spline& src)
{
	
	n_base=src.n_base;
	x_base= static_cast<double*>(malloc(n_base*sizeof(double)));
	y_base= static_cast<double*>(malloc(n_base*sizeof(double)));
	y2 = static_cast<double*>(malloc((n_base+1)*sizeof(double)));
	u = static_cast<double*>(malloc((n_base+1)*sizeof(double)));
	_min=src._min;
	_max=src._max;
	for(unsigned int i=0; i<n_base;i++){
		x_base[i]=src.x_base[i];
		y_base[i]=src.y_base[i];
		y2[i]=src.y2[i];
	}
	y2[n_base]=src.y2[n_base];
	for(unsigned int i=0; i<n_base-1;i++){
		u[i]=src.u[i];
	}
	return *this;
}

Spline::~Spline ( )
{
	if(n_base){
		free(u);
		//free(pp);
		free(y2);
		free(y_base);
		free(x_base);
	}
}

/**
 * @param  xvalues returns the abscissae of the base point of the spline
 * @param  yvalues returns the ordinates of the base point of the spline
 */
void Spline::get_base_points (doubleVector& xvalues, doubleVector& yvalues ) const
{
	xvalues.clear();
	yvalues.clear();
	for(unsigned int i=0;i<n_base;i++){
		xvalues.push_back(x_base[i]);
		yvalues.push_back(y_base[i]);
	}
	return;
}

double Spline::evaluate (double v )
{
	int klo,khi,k;
	double h,b,a;
	if (v < _min || v > _max ) throw std::range_error("input value out of range");
	
	klo=0;
	khi=n_base-1; //era n_base !!!!!
	while (khi-klo > 1) {
		k=(khi+klo) >> 1;//k=(khi+klo)/2;
		if (x_base[k] > v) khi=k;
		else klo=k;
	}
	h=x_base[khi]-x_base[klo];
	if (h == 0.0) throw std::range_error("input value out of range");
	a=(x_base[khi]-v)/h;
	b=(v-x_base[klo])/h;
	return a*y_base[klo]+b*y_base[khi]+((a*a*a-a)*y2[klo]+(b*b*b-b)*y2[khi])*(h*h)/6.0;
}

doubleVector Spline::evaluate (doubleVector in_val )
{
	//simplicistic implementation. Should make better use of the indexing
	doubleVector outval;
	for(unsigned int i=0; i <  in_val.size(); i++) outval.push_back(evaluate(in_val[i]));
	return outval;
}


// bool Spline::bracket(double goal,double initial,double tol, double& xlow, double& xhigh)
// {
// 	if(tol<=std::numeric_limits<double>::epsilon()) throw std::range_error("bracket: tol too small");
// 	const double fact=1.618;
// 	const unsigned int ntry=50;
// 	double x1,x2;
// 	x1=initial-tol; if(x1 < _min) x1=_min;
// 	x2=initial+tol; if(x2 > _max) x2=_max;
// 	double f1,f2;
// 	f1=goal - evaluate(x1);
// 	f2=goal - evaluate(x2);
// 	for(unsigned int j=0; j < ntry; j++){
// 		if(f1*f2 <0.0){
// 			xlow=x1;
// 			xhigh=x2;
// 			return true;
// 		}
// 		if(fabs(f1) < fabs(f2)){
// 			x1 += fact*(x1-x2);
// 			if(x1 < _min) x1=_min;
// 			if(x1 > _max) x1=_max;
// 			f1=goal - evaluate(x1);
// 		}
// 		else{
// 			x2 += fact *(x2-x1);
// 			if(x2 < _min) x2=_min;
// 			if(x2 > _max) x2=_max;
// 			f2=goal - evaluate(x2);
// 		}
// 	}
// 	return false;
// }
// 
// 
// double Spline::solve(double goal,double x1,double x2,double tol,unsigned int maxiter)
// {
// 	double fl,fh,xl,xh,del,f,rtf,dx;
// 	fl = goal - evaluate(x1);
// 	fh = goal - evaluate(x2);
// 	if( fl*fh > 0.0) throw std::range_error("solve: initial interval does not braket solution");
// 	if(fl < 0.0 ){
// 		xl=x1;
// 		xh=x2;
// 	}
// 	else{
// 		xl=x2;
// 		xh=x1;
// 		double swap=fl;
// 		fl=fh;
// 		fh=swap;
// 	}
// 	dx=xh-xl;
// 	for(unsigned int j=0; j < maxiter; j++){
// 		rtf=xl+dx*fl/(fl-fh);
// 		f= goal - evaluate(rtf);
// 		if(f < 0.0){
// 			del=xl-rtf;
// 			xl=rtf;
// 			fl=f;
// 		}
// 		else{
// 			del=xh-rtf;
// 			xh=rtf;
// 			fh=f;
// 		}
// 		dx=xh-xl;
// 		if(fabs(del) < tol || f == 0.0) return rtf;
// 	}
// 	return NAN; //maxiter exceeded
// }

} //close namespace Interpolator