#include <../source/TAL/objbase.h>

/*
#ifdef  CONFIG_MATH_SPLINE
*/

/*
 * One-dimensional spline creation / interpolation routines.
 * Copyright (C) 1988 - 1992 by Numerical Redpes Software.
 * Minor hacks by Vladimir A. Smirnov.
 */

/*
 * Given arrays x[1..n] and y[1..n] containing a tabulated function,
 * i.e., yi = f(xi), with x1<x2< ... <xN , and given values yp1 and ypn
 * for the first derivative of the interpolating function at points
 * 1 and n, respectively, this routine returns an array y2[1..n] that
 * contains the second derivatives of the interpolating function at the
 * tabulated points xi.
 *
 * If l_natural / r_natural is set to 'true', the routine is signaled to
 * set the corresponding boundary condition for a natural spline, with
 * zero second derivative on that boundary.
 */
V_LOCAL void
nr_spline(mvec_t x[],mvec_t y[],u32_t n,mvec_t yp1,mvec_t ypn,mvec_t y2[],
	vboolean l_natural, vboolean r_natural)
{
	u32_t i, k;
	mvec_t p, qn, sig, un, *u;
	u = (mvec_t*)memAlloc(n * sizeof(mvec_t));
	memZero(u,n * sizeof(mvec_t));
	if (l_natural)
		y2[1]=u[1]=0.0;
	else
	{
		y2[1] = -0.5;
		u[1]=((mvec_t)3/(x[2]-x[1]))*((y[2]-y[1])/(x[2]-x[1])-yp1);
	}
	for(i=2;i<=n-1;i++)
	{
		/*
		 * This is the decomposition loop of the tridiagonal algorithm.
		 * y2 and u are used for temporary storage of the decomposed
		 * factors.
		 */
		 sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
		 p=sig*y2[i-1]+(mvec_t)2;
		 y2[i]=(sig-(mvec_t)1)/p;
		 u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		 u[i]=((mvec_t)6*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
	}
	if (r_natural)
		qn=un=0.0;
	else
	{
		qn=(mvec_t)0.5;
		un=((mvec_t)3/(x[n]-x[n-1]))*(ypn-(y[n]-y[n-1])/(x[n]-x[n-1]));
	}
	y2[n]=(un-qn*u[n-1])/(qn*y2[n-1]+(mvec_t)1);
	for (k=n-1;k>=1;k--)
		/*
		 * This is the backsubstitution loop of the tridiagonal algorithm.
		 */
		y2[k]=y2[k]*y2[k+1]+u[k];
	memFree(u);
}

/*
 * ORIGINAL:
 * Given the arrays xa[1..n] and ya[1..n], which tabulate a function
 * (with the xai's in order), and given the array y2a[1..n], which is
 * the output from nr_spline() above, and given a value of x, this routine
 * returns a cubic-spline interpolated value y.
 */
V_LOCAL mvec_t
nr_splint(mvec_t xa[],mvec_t ya[],mvec_t y2a[],u32_t n,mvec_t x,u32_t *prev_klo)
{
	u32_t klo, khi, k;
	mvec_t h, rh, b, a;
	const mvec_t inv_6 = (mvec_t)1 / (mvec_t)6;
	/*
	 * ORIGINAL:
	 * We will find the right place in the table by means of
	 * bisection. This is optimal if sequential calls to this
	 * routine are at random values of x. If sequential calls
	 * are in order, and closely spaced, one would do better
	 * to store previous values of klo and khi and test if
	 * they remain appropriate on the next call.
	 */
	/*
	 * Ok; let's implement this.
	 */
	klo=*prev_klo;
	if(xa[klo] > x || xa[klo+1] < x)
	{
		klo=1;
		khi=n;
		while (khi-klo > 1)
		{
			k=(khi+klo) >> 1;
			if (xa[k] > x)
				khi=k;
			else
				klo=k;
		}
		*prev_klo = klo;
	}
	else
		khi = klo + 1;
	/*
	 * klo and khi now bracket the input value of x.
	 */
	h=xa[khi]-xa[klo];
	rh = 1 / h;
	a=(xa[khi]-x) * rh;
	b=(x-xa[klo]) * rh;
	return a*ya[klo]+b*ya[khi]+
		((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)*inv_6;
}

/*
 * Wrapper.
 */
typedef struct cspline_s
{
	libv_object_t   base;
	u32_t           n, prev_klo;
	vboolean		ln, rn;
	mvec_t          yp0, ypn;
	mvec_t          *xa, *ya, *y2;
}   cspline_t, *cspline_p;

#ifdef  CHECKED_BUILD
#define CSP_MAGIC                       0x4453504C      /* CSPL */
V_LOCAL char_p  csp_object      = "cubic spline";
#endif  /* CHECKED_BUILD */

V_LOCAL u32_t V_CALL
csp_destroy(vhandle_t handle)
{
	cspline_p self=(cspline_p)handle;
	if(--self->base.ref_count)
		return self->base.ref_count;
	memFreeObject(self);
	return 0;
}

#ifndef CHECKED_BUILD
#define cspCheckVX(vx,caller)   __cspCheckVX(vx)
V_LOCAL vboolean
__cspCheckVX(vector_t *vx)
#else   /* CHECKED_BUILD */
V_LOCAL vboolean
cspCheckVX(vector_t *vx,char *caller)
#endif  /* CHECKED_BUILD */
{
	int i,n=vx->size;
	/*
	 * The xa's must be distinct (and incremental)
	 */
	for(i=1;i<n;i++)
	{
		if(vx->elements[i] - vx->elements[i-1] < FLOAT_ZERO_EPSILON)
		{
#ifdef  CHECKED_BUILD
			conlPrintf(LOG_ERROR,"%s(): malformed input:"
				" vx(%u)=%g, but vx(%u)=%g.\n",caller,
				i-1,vx->elements[i-1],i,vx->elements[i]);
#endif  /* CHECKED_BUILD */
			return false;
		}
	}
	return true;
}

vhandle_t V_GLOBAL
cspCreate(vector_t *vx,vector_t *vy)
{
	u32_t n;
	cspline_t *sp;
	if(!vx || !vy)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"cspCreate(%X,%X): incorrect parameter(s).\n",vx,vy);
#endif  /* CHECKED_BUILD */
		return NULL;
	}
	n=vx->size;
	if(n!=vy->size)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"cspCreate(): vx->size=%u, but vy->size=%u.\n",
			vx->size,vy->size);
#endif  /* CHECKED_BUILD */
		return NULL;
	}
	if(!cspCheckVX(vx,"cspCreate"))
		return NULL;
	ALLOC_OBJECT(sp,cspline_t,n * 3 * sizeof(mvec_t),CSP_MAGIC);
	sp->base.destroy = csp_destroy;
	sp->n = n;
	sp->prev_klo = 1;
	sp->ln = sp->rn = true;
	sp->yp0 = sp->ypn = 0;
	/* Arrays to pass to nr_spliXXX() must be unit - based */
	sp->xa = (mvec_t*)(sp + 1) - 1;
	sp->ya = sp->xa + n;
	sp->y2 = sp->ya + n;
	memCopy(sp->xa+1, vx->elements, n * sizeof(mvec_t));
	memCopy(sp->ya+1, vy->elements, n * sizeof(mvec_t));
	nr_spline(sp->xa,sp->ya,sp->n,sp->yp0,sp->ypn,sp->y2,sp->ln,sp->rn);
	return (vhandle_t)sp;
}

mvec_t  V_GLOBAL
cspInterpolate(vhandle_t handle,mvec_t x)
{
	/* FIXME: libvIsHandle() may take too long time to execute */
	cspline_p sp=(cspline_p)libvIsHandle(handle,
		"cspInterpolate",csp_object,CSP_MAGIC);
	if(sp == NULL)
	return 0;
	return nr_splint(sp->xa, sp->ya, sp->y2, sp->n, x, &sp->prev_klo);
}

void    V_GLOBAL
cspChangeX(vhandle_t handle,vector_t *new_x)
{
	cspline_p sp=(cspline_p)libvIsHandle(handle,
		"cspChangeX",csp_object,CSP_MAGIC);
	if(sp == NULL)
	return;
	if(sp->n != new_x->size)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,"cspChangeX(%X,%X): expected size of vector "
			"'%d', found '%d'.\n",handle,new_x,sp->n,new_x->size);
#endif  /* CHECKED_BUILD */
		return;
	}
	if(!cspCheckVX(new_x,"cspChangeX"))
		return;
	memCopy(sp->xa+1,new_x->elements,sp->n * sizeof(mvec_t));
	sp->prev_klo=1;
	nr_spline(sp->xa,sp->ya,sp->n,sp->yp0,sp->ypn,sp->y2,sp->ln,sp->rn);
}

void    V_GLOBAL
cspChangeY(vhandle_t handle,vector_t *new_y)
{
	cspline_p sp=(cspline_p)libvIsHandle(handle,
		"cspChangeY",csp_object,CSP_MAGIC);
	if(sp == NULL)
	return;
	if(sp->n != new_y->size)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,"cspChangeY(%X,%X): expected size of vector "
			"'%d', found '%d'.\n",handle,new_y,sp->n,new_y->size);
#endif  /* CHECKED_BUILD */
		return;
	}
	memCopy(sp->ya+1,new_y->elements,sp->n * sizeof(mvec_t));
	sp->prev_klo=1;
	nr_spline(sp->xa,sp->ya,sp->n,sp->yp0,sp->ypn,sp->y2,sp->ln,sp->rn);
}

void    V_GLOBAL
cspSetBoundaryCondition(vhandle_t handle,
	vboolean zero_second_derivative,mvec_t f_deriv_l,mvec_t f_deriv_r)
{
	cspline_p sp=(cspline_p)libvIsHandle(handle,
		"cspSetBoundaryCondition",csp_object,CSP_MAGIC);
	if(sp == NULL)
	return;
	if(zero_second_derivative)
	{
		sp->ln = sp->rn = true;
		sp->yp0 = sp->ypn = 0;
	}
	else
	{
		sp->ln = sp->rn = false;
		sp->yp0 = f_deriv_l;
		sp->ypn = f_deriv_r;
	}
	sp->prev_klo=1;
	nr_spline(sp->xa,sp->ya,sp->n,sp->yp0,sp->ypn,sp->y2,sp->ln,sp->rn);
}

void    V_GLOBAL
cspDestroy(vhandle_t handle)
{
	libvCloseHandle(handle,"cspDestroy",csp_object,CSP_MAGIC);
}

/*
#endif  // CONFIG_MATH_SPLINE
*/
