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

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

/*
 * Begin of NR code ******************************************************
 */

/*
 * 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(vec_t x[],vec_t y[],u32_t n,vec_t yp1,vec_t ypn,vec_t y2[],
	vboolean l_natural, vboolean r_natural)
{
	u32_t i, k;
	vec_t p, qn, sig, un, *u;
	u = (vec_t*)memAlloc(n * sizeof(vec_t));
	memZero(u,n * sizeof(vec_t));
	if (l_natural)
		y2[1]=u[1]=0.0;
	else
	{
		y2[1] = -0.5;
		u[1]=((vec_t)3/(x[2]-x[1])) * ((y[2]-y[1])/(x[2]-x[1])-yp1);
	}
	for(i=2;i<n;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]+(vec_t)2;
		 y2[i]=(sig-(vec_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]=((vec_t)6*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
	}
	if (r_natural)
		qn=un=0.0;
	else
	{
		qn=(vec_t)0.5;
		un=((vec_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]+(vec_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);
}

/*
 * 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 vec_t
nr_splint(vec_t xa[],vec_t ya[],vec_t y2a[],u32_t n,vec_t x,u32_t *prev_klo)
{
	u32_t klo, khi, k;
	vec_t h, rh, b, a;
	const vec_t inv_6 = (vec_t)1 / (vec_t)6;
	/*
	 * 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;
}

/*
 * End of NR code ********************************************************
 */
 
/*
 * Wrapper for one-dimensional spline.
 */
typedef struct cspline_s
{
	libv_object_t   base;
	u32_t		   n;			/* Number of points in spline */
	u32_t			prev_klo;	/* Previous index of min{xa[] < x} - used in heuristic */
	vboolean		ln, rn;		/* If true, use natural boundary conditions on that endpoint */
	vec_t		  yp0, ypn;	/* User-supplied 1st derivatives at the endpoints */
	vec_t		  *xa;		/* User-supplied array x (independent variable) */
	vec_t			*ya;		/* User-supplied array y (function) */
	vec_t			*y2;		/* Generated array of 2nd derivatives */
}   cspline_t, *cspline_p;

#ifdef  CHECKED_BUILD
#define CSP_MAGIC			   0x44535031	  /* CSP1 */
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;
	UNREF_EXIT(self);
	memFreeObject(self);
	return 0;
}

#ifndef CHECKED_BUILD
#define cspCheckVXVec(X,size,caller)   __cspCheckVXVec(X,size)
V_LOCAL vboolean
__cspCheckVXVec(vec_p X,u32_t size)
#else   /* CHECKED_BUILD */
V_LOCAL vboolean
cspCheckVXVec(vec_p X,u32_t size,char *caller)
#endif  /* CHECKED_BUILD */
{
	u32_t i;
	/*
	 * X values must be distinct and incremental
	 */
	for(i = 1;i < size;i++)
	{
		if(X[i] - X[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,X[i-1],i,X[i]);
#endif  /* CHECKED_BUILD */
			return false;
		}
	}
	return true;
}

vhandle_t V_GLOBAL
cspVecCreate(vec_p X,vec_p Y,u32_t num_points)
{
	cspline_t *sp;
	if(!X || !Y || !num_points)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"cspVecCreate(%X,%X,%u): incorrect parameter(s).\n",X,Y,num_points);
#endif  /* CHECKED_BUILD */
		return LIBV_INVALID_HANDLE;
	}
	if(!cspCheckVXVec(X,num_points,"cspVecCreate"))
		return LIBV_INVALID_HANDLE;
	ALLOC_OBJECT(sp,cspline_t,num_points * 3 * sizeof(vec_t),CSP_MAGIC);
	sp->base.destroy = csp_destroy;
	sp->n = num_points;
	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 = (vec_t*)(sp + 1) - 1;
	sp->ya = sp->xa + num_points;
	sp->y2 = sp->ya + num_points;
	memCopy(sp->xa+1, X, num_points * sizeof(vec_t));
	memCopy(sp->ya+1, Y, num_points * sizeof(vec_t));
	nr_spline(sp->xa,sp->ya,sp->n,sp->yp0,sp->ypn,sp->y2,sp->ln,sp->rn);
	return (vhandle_t)sp;
}

vhandle_t V_GLOBAL
cspCreate(vector_t *vx,vector_t *vy)
{
	if(!vx || !vy)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"cspCreate(%X,%X): incorrect parameter(s).\n",vx,vy);
#endif  /* CHECKED_BUILD */
		return LIBV_INVALID_HANDLE;
	}
	if(vx->size != 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 LIBV_INVALID_HANDLE;
	}
	return cspVecCreate(vx->elements, vy->elements, vx->size);
}

vec_t  V_GLOBAL
cspInterpolate(vhandle_t handle,vec_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)
		/*
		 * No way to indicate error;
		 * choose 1 because it probably won't cause zero-didvide :)
		 */
		return 1;
	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(!cspCheckVXVec(new_x->elements,new_x->size,"cspChangeX"))
		return;
	memCopy(sp->xa+1,new_x->elements,sp->n * sizeof(vec_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(vec_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,vec_t f_deriv_l,vec_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);
}

/*
 * Wrapper for N-dimensional parametric curve.
 * Periodic boundary conditions currently not implemented, so
 * for closed shapes we come with hack: repeat end points at the
 * beginning, and start points at the and.
 * Note also that for 3D curves boundary conditions can be either
 * natural (zero second derivatives at the end points) in case of
 * non-closed shape, or 'periodic' in case of closed one.
 */

typedef struct csplineNd_s
{
	libv_object_t   base;
	/* Data shared for all 3 splines ---------------------------------- */
	u32_t		   n;			/* Number of points in spline */
	u32_t		   real_n;		/* Number of points in spline + 4 points for hack */
	u32_t			ndim;		/* Dimension */
	vboolean		closed;		/* If true, use 'periodic hack' */
	u32_t			prev_klo;	/* min{ta[] < t} used in heuristic */
	vec_t		  *ta;		/* User-supplied parameter values */
	/* Parametric representation -------------------------------------- */
	vec_t			**xa;		/* User-supplied arrays of x[0]..x[ndim] */
	vec_t			**x2;		/* Generated array of 2nd derivatives */
}	csplineNd_t, *csplineNd_p;

#ifdef  CHECKED_BUILD
#define CSPN_MAGIC			   0x4453504E		/* CSPN */
V_LOCAL char_p cspn_object	   = "parametric curve";
#endif  /* CHECKED_BUILD */

#define	SAMEPOINT_THRESHOLD		FLOAT_ZERO_EPSILON

V_LOCAL vboolean ndim_samepoint(vector_t *vx[],u32_t ndim)
{
	u32_t i,last;
	last = vx[0]->size-1;
	for(i=0;i<ndim;i++)
	{
		if(v_abs(vx[i]->elements[0]-vx[i]->elements[last]) > SAMEPOINT_THRESHOLD)
			return false;
	}
	return true;
}

V_LOCAL u32_t V_CALL
cspn_destroy(vhandle_t handle)
{
	csplineNd_p self = (csplineNd_p)handle;
	UNREF_EXIT(self);
	memFreeObject(self);
	return 0;
}

vhandle_t V_GLOBAL
cspNdCreate(vector_t *vt,vector_t *vx[],u32_t ndim)
{
	u32_t i,n,real_n;
	u8_t *ptr;
	csplineNd_t *spn;
	u32_t a_store_cksize, xa_cksize, cksize_extra;
	vec_t t_delta_beg, t_delta_end;
	vboolean curve_closed;
	if(!vt || !vx || !ndim || vt->size<3)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"cspNdCreate(vt=%X,vx=%X,ndim=%u;vt->size=%u):"
				" incorrect parameter(s).\n",vt,vx,ndim,vt->size);
#endif  /* CHECKED_BUILD */
		return LIBV_INVALID_HANDLE;
	}
	n=vt->size;
	for(i=0;i<ndim;i++)
	{
		if(!vx[i])
		{
#ifdef  CHECKED_BUILD
			conlPrintf(LOG_ERROR,"cspNdCreate(vt=%X,vx=%X,ndim=%u):"
				" NULL vector at %u of vx[].\n",vt,vx,ndim,i);
#endif  /* CHECKED_BUILD */
			return LIBV_INVALID_HANDLE;
		}
		if(n!=vx[i]->size)
		{
#ifdef  CHECKED_BUILD
			conlPrintf(LOG_ERROR,"cspNdCreate(vt=%X,vx=%X,ndim=%u):"
				" vt->size=%u, but vx[%u]->size=%u.\n",
				vt,vx,ndim,n,i,vx[i]->size);
#endif  /* CHECKED_BUILD */
			return LIBV_INVALID_HANDLE;
		}
	}
	if(!cspCheckVXVec(vt->elements,vt->size,"cspNdCreate"))
		return LIBV_INVALID_HANDLE;
	/*
	 * Compute amount of memory for all arrays
	 */
	curve_closed = ndim_samepoint(vx,ndim);
	if(curve_closed)
		real_n = n + 4;	/* primitive 'periodic' hack */
	else
		real_n = n;
	a_store_cksize = real_n * sizeof(vec_t);	/* ta, xa[i], x2[i] */
	xa_cksize = ndim * sizeof(vec_t*);			/* xa, x2 */
	cksize_extra = 2 * xa_cksize + a_store_cksize * (1 + 2 * ndim);
	ALLOC_OBJECT(spn,csplineNd_t,cksize_extra,CSPN_MAGIC);
	spn->closed = curve_closed;
	spn->n = n;
	spn->real_n = real_n;
	spn->ndim = ndim;
	spn->base.destroy = cspn_destroy;
	spn->prev_klo = 1;
	ptr = (u8_t*)(spn+1);
	spn->xa = (vec_t**)ptr;
	ptr += xa_cksize;
	spn->x2 = (vec_t**)ptr;
	ptr += xa_cksize;
	spn->ta = (vec_t*)(ptr - sizeof(vec_t)); /* unit-based array */
	if(curve_closed)
	{
		/* Shift data, extend parameter range */
		memCopy(&spn->ta[3],vt->elements,n * sizeof(vec_t));
		t_delta_beg = vt->elements[1] - vt->elements[0];
		t_delta_end = vt->elements[n-1] - vt->elements[n-2];
		spn->ta[2] = spn->ta[3] - t_delta_beg;
		spn->ta[1] = spn->ta[2] - t_delta_beg;
		spn->ta[real_n - 1] = spn->ta[real_n - 2] + t_delta_end;
		spn->ta[real_n] = spn->ta[real_n - 1] + t_delta_end;
		for(i=0;i<ndim;i++)
		{
			ptr += a_store_cksize;
			spn->xa[i] = (vec_t*)(ptr - sizeof(vec_t)); /* unit-based array */
			ptr += a_store_cksize;
			spn->x2[i] = (vec_t*)(ptr - sizeof(vec_t)); /* unit-based array */
			memCopy(&spn->xa[i][3],vx[i]->elements,n * sizeof(vec_t));
			/* Repeat end points at the beginning */
			spn->xa[i][1] = vx[i]->elements[n-3];
			spn->xa[i][2] = vx[i]->elements[n-2];
			/* Repeat start points at the end */
			spn->xa[i][real_n - 1] = vx[i]->elements[1];
			spn->xa[i][real_n] = vx[i]->elements[2];
			nr_spline(spn->ta,spn->xa[i],real_n,0,0,spn->x2[i],true,true);
		}
	}
	else
	{
		memCopy(ptr,vt->elements,n * sizeof(vec_t));
		for(i=0;i<ndim;i++)
		{
			ptr += a_store_cksize;
			spn->xa[i] = (vec_t*)ptr - 1; /* unit-based array */
			memCopy(ptr,vx[i]->elements,n * sizeof(vec_t));
			ptr += a_store_cksize;
			spn->x2[i] = (vec_t*)ptr - 1; /* unit-based array */
			nr_spline(spn->ta,spn->xa[i],spn->n,0,0,spn->x2[i],true,true);
		}
	}
	return (vhandle_t)spn;
}

void V_GLOBAL
cspNdInterpolate(vhandle_t handle,vec_t t,vec_t *xa_ret)
{
	u32_t i;
	/* FIXME: libvIsHandle() may take too long time to execute */
	csplineNd_p spn=(csplineNd_p)libvIsHandle(handle,
		"cspNdInterpolate",cspn_object,CSPN_MAGIC);
	if(spn == NULL)
		return;
	for(i=0;i<spn->ndim;i++)
		xa_ret[i]=nr_splint(spn->ta,spn->xa[i],spn->x2[i],spn->real_n,t,&spn->prev_klo);
}

void	V_GLOBAL
cspNdChangeX(vhandle_t handle,vector_t *new_x[])
{
	u32_t i;
	csplineNd_p spn=(csplineNd_p)libvIsHandle(handle,
		"cspNdChangeX",cspn_object,CSPN_MAGIC);
	if(!new_x || !spn)
		return;
	if(spn->n != new_x[0]->size)
	{
#ifdef  CHECKED_BUILD
		conlPrintf(LOG_ERROR,"cspNdChangeX(%X,%X): expected size of vector "
			"'%d', found '%d'.\n",handle,new_x,spn->n,new_x[0]->size);
#endif  /* CHECKED_BUILD */
		return;
	}
	if(spn->closed)
	{
		u32_t real_n = spn->n + 4;
		spn->prev_klo = 1;
		for(i=0;i<spn->ndim;i++)
		{
			memCopy(&spn->xa[i][3],new_x[i]->elements,spn->n * sizeof(vec_t));
			/* Repeat end points at the beginning */
			spn->xa[i][1] = new_x[i]->elements[spn->n-3];
			spn->xa[i][2] = new_x[i]->elements[spn->n-2];
			/* Repeat start points at the end */
			spn->xa[i][real_n - 1] = new_x[i]->elements[1];
			spn->xa[i][real_n] = new_x[i]->elements[2];
			nr_spline(spn->ta,spn->xa[i],real_n,0,0,spn->x2[i],true,true);
		}
	}
	else
	{
		spn->prev_klo=1;
		for(i=0;i<spn->ndim;i++)
		{
			memCopy(&spn->xa[i][1],new_x[i]->elements,spn->n * sizeof(vec_t));
			nr_spline(spn->ta,spn->xa[i],spn->n,0,0,spn->x2[i],true,true);
		}
	}
}

void	V_GLOBAL
cspNdDestroy(vhandle_t handle)
{
	libvCloseHandle(handle,"cspNdDestroy",cspn_object,CSPN_MAGIC);
}
