/* ImLib3D
 * Copyright (c) 2001, ULP-IPB Strasbourg.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
/*! \file SplineInterpolation.hxx */

#ifndef _SplineInterpolation_hxx
#define _SplineInterpolation_hxx

#include<ImLib3D/Vect3D.hpp>
#include<ImLib3D/TaskProgressManager.hpp>
#include<float.h>

// *************************** Value ****************************
//! compute value at non integer position using B-spline interpolation
template<class Im3DValue>
Im3DValue
SplineInterpolator3D<Im3DValue>::Value(const ImageType &ima,const float x,const float y,const float z)
{
	if(&ima != srcImage)
	{
		ThrowError("SplineInterpolator3D::Value incoherence. Did you  use \"Initialize\"");
	}
	const ImageType &Ccoeff=*coeffImage;

	int Width =Ccoeff.Width ();
	int Height=Ccoeff.Height();
	int Depth =Ccoeff.Depth ();

 	int r=(splineDegree+1)/2;
	if(x>=r && y>=r && z>=r && x<=(Width-r-1) && y<=(Height-r-1) && z<=(Depth-r-1))
  	{
  		DBGvaluefastct++;
  		return ValueFast(Ccoeff,x,y,z);
  	}
  	if(boundary==BOUNDARY_ZERO && !Ccoeff.IsInside(x,y,z))
  	{
  		return Ccoeff.GetZero();
  	}
	if(boundary==BOUNDARY_BACKGROUND && !Ccoeff.IsInside(x,y,z))
	{
		ThrowError("SplineInterpolation BACKGROUND UNIMPLEMENTED");
//  		return *Ccoeff.properties.background;
	}
	DBGvaluect++;
	Scalar	xWeight[6], yWeight[6], zWeight[6];
	Scalar	interpolated;
	Scalar	w, w2, w4, t, t0, t1;
	long	xIndex[6], yIndex[6], zIndex[6];
	long	Width2 = 2L * Width - 2L, Height2 = 2L * Height - 2L, Depth2 = 2L * Depth - 2L;

	/* compute the interpolation indexes */
	{
		long	i, j, q, k;
		if (splineDegree & 1L) 
		{
			i = (long)floor(x) - splineDegree / 2L;
			j = (long)floor(y) - splineDegree / 2L;
			q = (long)floor(z) - splineDegree / 2L;
		}
		else 
		{
			i = (long)floor(x + 0.5) - splineDegree / 2L;
			j = (long)floor(y + 0.5) - splineDegree / 2L;
			q = (long)floor(z + 0.5) - splineDegree / 2L;
		}
		for (k = 0L; k <= splineDegree; k++) 
		{
			xIndex[k] = i++;
			yIndex[k] = j++;
			zIndex[k] = q++;
		}
	}

	/* compute the interpolation weights */
	switch (splineDegree) 
	{
	case 2:
		/* x */
		w = x - (Scalar)xIndex[1];
		xWeight[1] = 3.0 / 4.0 - w * w;
		xWeight[2] = (1.0 / 2.0) * (w - xWeight[1] + 1.0);
		xWeight[0] = 1.0 - xWeight[1] - xWeight[2];
		/* y */
		w = y - (Scalar)yIndex[1];
		yWeight[1] = 3.0 / 4.0 - w * w;
		yWeight[2] = (1.0 / 2.0) * (w - yWeight[1] + 1.0);
		yWeight[0] = 1.0 - yWeight[1] - yWeight[2];
		/* z */
		w = z - (Scalar)zIndex[1];
		zWeight[1] = 3.0 / 4.0 - w * w;
		zWeight[2] = (1.0 / 2.0) * (w - zWeight[1] + 1.0);
		zWeight[0] = 1.0 - zWeight[1] - zWeight[2];
		break;
	case 3:
		/* x */
		w = x - (Scalar)xIndex[1];
		xWeight[3] = (1.0 / 6.0) * w * w * w;
		xWeight[0] = (1.0 / 6.0) + (1.0 / 2.0) * w * (w - 1.0) - xWeight[3];
		xWeight[2] = w + xWeight[0] - 2.0 * xWeight[3];
		xWeight[1] = 1.0 - xWeight[0] - xWeight[2] - xWeight[3];
		/* y */
		w = y - (Scalar)yIndex[1];
		yWeight[3] = (1.0 / 6.0) * w * w * w;
		yWeight[0] = (1.0 / 6.0) + (1.0 / 2.0) * w * (w - 1.0) - yWeight[3];
		yWeight[2] = w + yWeight[0] - 2.0 * yWeight[3];
		yWeight[1] = 1.0 - yWeight[0] - yWeight[2] - yWeight[3];
		/* z */
		w = z - (Scalar)zIndex[1];
		zWeight[3] = (1.0 / 6.0) * w * w * w;
		zWeight[0] = (1.0 / 6.0) + (1.0 / 2.0) * w * (w - 1.0) - zWeight[3];
		zWeight[2] = w + zWeight[0] - 2.0 * zWeight[3];
		zWeight[1] = 1.0 - zWeight[0] - zWeight[2] - zWeight[3];
		break;
	case 4:
		/* x */
		w = x - (Scalar)xIndex[2];
		w2 = w * w;
		t = (1.0 / 6.0) * w2;
		xWeight[0] = 1.0 / 2.0 - w;
		xWeight[0] *= xWeight[0];
		xWeight[0] *= (1.0 / 24.0) * xWeight[0];
		t0 = w * (t - 11.0 / 24.0);
		t1 = 19.0 / 96.0 + w2 * (1.0 / 4.0 - t);
		xWeight[1] = t1 + t0;
		xWeight[3] = t1 - t0;
		xWeight[4] = xWeight[0] + t0 + (1.0 / 2.0) * w;
		xWeight[2] = 1.0 - xWeight[0] - xWeight[1] - xWeight[3] - xWeight[4];
		/* y */
		w = y - (Scalar)yIndex[2];
		w2 = w * w;
		t = (1.0 / 6.0) * w2;
		yWeight[0] = 1.0 / 2.0 - w;
		yWeight[0] *= yWeight[0];
		yWeight[0] *= (1.0 / 24.0) * yWeight[0];
		t0 = w * (t - 11.0 / 24.0);
		t1 = 19.0 / 96.0 + w2 * (1.0 / 4.0 - t);
		yWeight[1] = t1 + t0;
		yWeight[3] = t1 - t0;
		yWeight[4] = yWeight[0] + t0 + (1.0 / 2.0) * w;
		yWeight[2] = 1.0 - yWeight[0] - yWeight[1] - yWeight[3] - yWeight[4];
		/* z */
		w = z - (Scalar)zIndex[2];
		w2 = w * w;
		t = (1.0 / 6.0) * w2;
		zWeight[0] = 1.0 / 2.0 - w;
		zWeight[0] *= zWeight[0];
		zWeight[0] *= (1.0 / 24.0) * zWeight[0];
		t0 = w * (t - 11.0 / 24.0);
		t1 = 19.0 / 96.0 + w2 * (1.0 / 4.0 - t);
		zWeight[1] = t1 + t0;
		zWeight[3] = t1 - t0;
		zWeight[4] = zWeight[0] + t0 + (1.0 / 2.0) * w;
		zWeight[2] = 1.0 - zWeight[0] - zWeight[1] - zWeight[3] - zWeight[4];
		break;
	case 5:
		/* x */
		w = x - (Scalar)xIndex[2];
		w2 = w * w;
		xWeight[5] = (1.0 / 120.0) * w * w2 * w2;
		w2 -= w;
		w4 = w2 * w2;
		w -= 1.0 / 2.0;
		t = w2 * (w2 - 3.0);
		xWeight[0] = (1.0 / 24.0) * (1.0 / 5.0 + w2 + w4) - xWeight[5];
		t0 = (1.0 / 24.0) * (w2 * (w2 - 5.0) + 46.0 / 5.0);
		t1 = (-1.0 / 12.0) * w * (t + 4.0);
		xWeight[2] = t0 + t1;
		xWeight[3] = t0 - t1;
		t0 = (1.0 / 16.0) * (9.0 / 5.0 - t);
		t1 = (1.0 / 24.0) * w * (w4 - w2 - 5.0);
		xWeight[1] = t0 + t1;
		xWeight[4] = t0 - t1;
		/* y */
		w = y - (Scalar)yIndex[2];
		w2 = w * w;
		yWeight[5] = (1.0 / 120.0) * w * w2 * w2;
		w2 -= w;
		w4 = w2 * w2;
		w -= 1.0 / 2.0;
		t = w2 * (w2 - 3.0);
		yWeight[0] = (1.0 / 24.0) * (1.0 / 5.0 + w2 + w4) - yWeight[5];
		t0 = (1.0 / 24.0) * (w2 * (w2 - 5.0) + 46.0 / 5.0);
		t1 = (-1.0 / 12.0) * w * (t + 4.0);
		yWeight[2] = t0 + t1;
		yWeight[3] = t0 - t1;
		t0 = (1.0 / 16.0) * (9.0 / 5.0 - t);
		t1 = (1.0 / 24.0) * w * (w4 - w2 - 5.0);
		yWeight[1] = t0 + t1;
		yWeight[4] = t0 - t1;
		/* z */
		w = z - (Scalar)zIndex[2];
		w2 = w * w;
		zWeight[5] = (1.0 / 120.0) * w * w2 * w2;
		w2 -= w;
		w4 = w2 * w2;
		w -= 1.0 / 2.0;
		t = w2 * (w2 - 3.0);
		zWeight[0] = (1.0 / 24.0) * (1.0 / 5.0 + w2 + w4) - zWeight[5];
		t0 = (1.0 / 24.0) * (w2 * (w2 - 5.0) + 46.0 / 5.0);
		t1 = (-1.0 / 12.0) * w * (t + 4.0);
		zWeight[2] = t0 + t1;
		zWeight[3] = t0 - t1;
		t0 = (1.0 / 16.0) * (9.0 / 5.0 - t);
		t1 = (1.0 / 24.0) * w * (w4 - w2 - 5.0);
		zWeight[1] = t0 + t1;
		zWeight[4] = t0 - t1;
		break;
	default:
		mprintf("Invalid spline degree\n");
		return(0.0);
	}

	/* apply the mirror boundary conditions */
	{
		int k;
		if (Width == 1L) 
		{
			for (k = 0L; k <= splineDegree; k++) 
			{
				xIndex[k] = 0L;
			}
		}
		if (Height == 1L) 
		{
			for (k = 0L; k <= splineDegree; k++) 
			{
				yIndex[k] = 0L;
			}
		}
		if (Depth == 1L) 
		{
			for (k = 0L; k <= splineDegree; k++) 
			{
				zIndex[k] = 0L;
			}
		}
		if(boundary==BOUNDARY_MIRROR || boundary==BOUNDARY_BACKGROUND)
		{// use mirror boundary conditions
			for (k = 0L; k <= splineDegree; k++) 
			{
				xIndex[k] = (xIndex[k] < 0L) 
					? (-xIndex[k] - Width2 * ((-xIndex[k]) / Width2))
					: ( xIndex[k] - Width2 * (  xIndex[k]  / Width2));
				if (Width <= xIndex[k]) 
				{
					xIndex[k] = Width2 - xIndex[k];
				}
				yIndex[k] = (yIndex[k] < 0L) 
					? (-yIndex[k] - Height2 * ((-yIndex[k]) / Height2))
					: ( yIndex[k] - Height2 * (  yIndex[k]  / Height2));
				if (Height <= yIndex[k]) 
				{
					yIndex[k] = Height2 - yIndex[k];
				}
				zIndex[k] = (zIndex[k] < 0L) 
					? (-zIndex[k] - Depth2 * ((-zIndex[k]) / Depth2))
					: ( zIndex[k] - Depth2 * (  zIndex[k]  / Depth2));
				if (Depth <= zIndex[k]) 
				{
					zIndex[k] = Depth2 - zIndex[k];
				}
			}
		}
		else
		{// use "spread" mirror conditions
			for (k = 0L; k <= splineDegree; k++) 
			{
				if(xIndex[k] <  0L    ){xIndex[k]=0;         }
				if(xIndex[k] >= Width ){xIndex[k]=Width - 1; }
				if(yIndex[k] <  0L    ){yIndex[k]=0;         }
				if(yIndex[k] >= Height){yIndex[k]=Height - 1;}
				if(zIndex[k] <  0L    ){zIndex[k]=0;         }
				if(zIndex[k] >= Depth ){zIndex[k]=Depth -  1;}
			}

		}
	}

#ifdef NOTDEF
	if(
		xIndex[0]>Width /3   && 
		yIndex[0]>Height/3   &&
		zIndex[0]>Depth /3   &&
		xIndex[0]<Width *2/3 &&
		yIndex[0]<Height*2/3 &&
		zIndex[0]<Depth *2/3     )
	{
		mprintf("POINT: %f %f %f\n",x,y,z);
		for (long i = 0L; i <= splineDegree; i++) 
		{
			mprintf("xWeight[%d]:%5.5f   ",i,xWeight[i]);
			mprintf("yWeight[%d]:%5.5f   ",i,yWeight[i]);
			mprintf("zWeight[%d]:%5.5f \n",i,zWeight[i]);
		}
		for (long i = 0L; i <= splineDegree; i++) 
		{
			mprintf("xIndex[%d]:%3d   ",i,xIndex[i]);
			mprintf("yIndex[%d]:%3d   ",i,yIndex[i]);
			mprintf("zIndex[%d]:%3d \n",i,zIndex[i]);
		}		
	}
#endif //NOTDEF
	/* perform interpolation */
	interpolated = 0.0;
	Im3DValue zero=0.0*Ccoeff(xIndex[0],yIndex[0],zIndex[0]);
	
	Im3DValue sumxyz=zero;
	for (long q = 0L; q <= splineDegree; q++) 
	{
		long z=zIndex[q];
		Im3DValue sumxy=zero;
		for (long j = 0L; j <= splineDegree; j++) 
		{
			Im3DValue sumx = zero;
			// beurk, mais sinon tres inefficace, faudrait creer un iterateur
			const Im3DValue *p=Ccoeff.GetData()+yIndex[j]*Width+z*Width*Height;
			for (long i = 0L; i <= splineDegree; i++) 
			{
				sumx += xWeight[i] * p[xIndex[i]];
			}
			sumxy += yWeight[j] * sumx;
		}
		sumxyz += zWeight[q] * sumxy;
	}

	return(sumxyz);
}

template<class Im3DValue>
void
SplineInterpolator3D<Im3DValue>::ComputeInterpolationWeights(Scalar *xWeight,Scalar *yWeight,Scalar *zWeight,
															 float x,float y,float z,
															 int x0,int y0,int z0)
{
	/* compute the interpolation weights */
	switch (splineDegree) 
	{
		Scalar	w, w2, w4, t, t0, t1;
	case 2:
		/* x */
		w = x - (Scalar)(x0+1);
		xWeight[1] = 3.0 / 4.0 - w * w;
		xWeight[2] = (1.0 / 2.0) * (w - xWeight[1] + 1.0);
		xWeight[0] = 1.0 - xWeight[1] - xWeight[2];
		/* y */
		w = y - (Scalar)(y0+1);
		yWeight[1] = 3.0 / 4.0 - w * w;
		yWeight[2] = (1.0 / 2.0) * (w - yWeight[1] + 1.0);
		yWeight[0] = 1.0 - yWeight[1] - yWeight[2];
		/* z */
		w = z - (Scalar)(z0+1);
		zWeight[1] = 3.0 / 4.0 - w * w;
		zWeight[2] = (1.0 / 2.0) * (w - zWeight[1] + 1.0);
		zWeight[0] = 1.0 - zWeight[1] - zWeight[2];
		break;
	case 3:
		/* x */
		w = x - (Scalar)(x0+1);
		xWeight[3] = (1.0 / 6.0) * w * w * w;
		xWeight[0] = (1.0 / 6.0) + (1.0 / 2.0) * w * (w - 1.0) - xWeight[3];
		xWeight[2] = w + xWeight[0] - 2.0 * xWeight[3];
		xWeight[1] = 1.0 - xWeight[0] - xWeight[2] - xWeight[3];
		/* y */
		w = y - (Scalar)(y0+1);
		yWeight[3] = (1.0 / 6.0) * w * w * w;
		yWeight[0] = (1.0 / 6.0) + (1.0 / 2.0) * w * (w - 1.0) - yWeight[3];
		yWeight[2] = w + yWeight[0] - 2.0 * yWeight[3];
		yWeight[1] = 1.0 - yWeight[0] - yWeight[2] - yWeight[3];
		/* z */
		w = z - (Scalar)(z0+1);
		zWeight[3] = (1.0 / 6.0) * w * w * w;
		zWeight[0] = (1.0 / 6.0) + (1.0 / 2.0) * w * (w - 1.0) - zWeight[3];
		zWeight[2] = w + zWeight[0] - 2.0 * zWeight[3];
		zWeight[1] = 1.0 - zWeight[0] - zWeight[2] - zWeight[3];
		break;
	case 4:
		/* x */
		w = x - (Scalar)(x0+2);
		w2 = w * w;
		t = (1.0 / 6.0) * w2;
		xWeight[0] = 1.0 / 2.0 - w;
		xWeight[0] *= xWeight[0];
		xWeight[0] *= (1.0 / 24.0) * xWeight[0];
		t0 = w * (t - 11.0 / 24.0);
		t1 = 19.0 / 96.0 + w2 * (1.0 / 4.0 - t);
		xWeight[1] = t1 + t0;
		xWeight[3] = t1 - t0;
		xWeight[4] = xWeight[0] + t0 + (1.0 / 2.0) * w;
		xWeight[2] = 1.0 - xWeight[0] - xWeight[1] - xWeight[3] - xWeight[4];
		/* y */
		w = y - (Scalar)(y0+2);
		w2 = w * w;
		t = (1.0 / 6.0) * w2;
		yWeight[0] = 1.0 / 2.0 - w;
		yWeight[0] *= yWeight[0];
		yWeight[0] *= (1.0 / 24.0) * yWeight[0];
		t0 = w * (t - 11.0 / 24.0);
		t1 = 19.0 / 96.0 + w2 * (1.0 / 4.0 - t);
		yWeight[1] = t1 + t0;
		yWeight[3] = t1 - t0;
		yWeight[4] = yWeight[0] + t0 + (1.0 / 2.0) * w;
		yWeight[2] = 1.0 - yWeight[0] - yWeight[1] - yWeight[3] - yWeight[4];
		/* z */
		w = z - (Scalar)(z0+2);
		w2 = w * w;
		t = (1.0 / 6.0) * w2;
		zWeight[0] = 1.0 / 2.0 - w;
		zWeight[0] *= zWeight[0];
		zWeight[0] *= (1.0 / 24.0) * zWeight[0];
		t0 = w * (t - 11.0 / 24.0);
		t1 = 19.0 / 96.0 + w2 * (1.0 / 4.0 - t);
		zWeight[1] = t1 + t0;
		zWeight[3] = t1 - t0;
		zWeight[4] = zWeight[0] + t0 + (1.0 / 2.0) * w;
		zWeight[2] = 1.0 - zWeight[0] - zWeight[1] - zWeight[3] - zWeight[4];
		break;
	case 5:
		/* x */
		w = x - (Scalar)(x0+2);
		w2 = w * w;
		xWeight[5] = (1.0 / 120.0) * w * w2 * w2;
		w2 -= w;
		w4 = w2 * w2;
		w -= 1.0 / 2.0;
		t = w2 * (w2 - 3.0);
		xWeight[0] = (1.0 / 24.0) * (1.0 / 5.0 + w2 + w4) - xWeight[5];
		t0 = (1.0 / 24.0) * (w2 * (w2 - 5.0) + 46.0 / 5.0);
		t1 = (-1.0 / 12.0) * w * (t + 4.0);
		xWeight[2] = t0 + t1;
		xWeight[3] = t0 - t1;
		t0 = (1.0 / 16.0) * (9.0 / 5.0 - t);
		t1 = (1.0 / 24.0) * w * (w4 - w2 - 5.0);
		xWeight[1] = t0 + t1;
		xWeight[4] = t0 - t1;
		/* y */
		w = y - (Scalar)(y0+2);
		w2 = w * w;
		yWeight[5] = (1.0 / 120.0) * w * w2 * w2;
		w2 -= w;
		w4 = w2 * w2;
		w -= 1.0 / 2.0;
		t = w2 * (w2 - 3.0);
		yWeight[0] = (1.0 / 24.0) * (1.0 / 5.0 + w2 + w4) - yWeight[5];
		t0 = (1.0 / 24.0) * (w2 * (w2 - 5.0) + 46.0 / 5.0);
		t1 = (-1.0 / 12.0) * w * (t + 4.0);
		yWeight[2] = t0 + t1;
		yWeight[3] = t0 - t1;
		t0 = (1.0 / 16.0) * (9.0 / 5.0 - t);
		t1 = (1.0 / 24.0) * w * (w4 - w2 - 5.0);
		yWeight[1] = t0 + t1;
		yWeight[4] = t0 - t1;
		/* z */
		w = z - (Scalar)(z0+2);
		w2 = w * w;
		zWeight[5] = (1.0 / 120.0) * w * w2 * w2;
		w2 -= w;
		w4 = w2 * w2;
		w -= 1.0 / 2.0;
		t = w2 * (w2 - 3.0);
		zWeight[0] = (1.0 / 24.0) * (1.0 / 5.0 + w2 + w4) - zWeight[5];
		t0 = (1.0 / 24.0) * (w2 * (w2 - 5.0) + 46.0 / 5.0);
		t1 = (-1.0 / 12.0) * w * (t + 4.0);
		zWeight[2] = t0 + t1;
		zWeight[3] = t0 - t1;
		t0 = (1.0 / 16.0) * (9.0 / 5.0 - t);
		t1 = (1.0 / 24.0) * w * (w4 - w2 - 5.0);
		zWeight[1] = t0 + t1;
		zWeight[4] = t0 - t1;
		break;
	default:
		ThrowError("Invalid spline degree\n");
	}
}


// *************************** ValueFast ****************************
//! compute value using B-spline interpolation, far from borders 
// this fct is speed optimized
template<class Im3DValue>
Im3DValue
SplineInterpolator3D<Im3DValue>::ValueFast(const ImageType &Ccoeff,const float x,const float y,const float z)
{
	int Width =Ccoeff.Width ();
	int Height=Ccoeff.Height();
//  	int Depth =Ccoeff.Depth ();

	Scalar	xWeight[6], yWeight[6], zWeight[6];
	Scalar	interpolated;
//  	long	xIndex[6], yIndex[6], zIndex[6];
	long x0,y0,z0;
//  	long	Width2 = 2 * Width - 2, Height2 = 2 * Height - 2, Depth2 = 2 * Depth - 2;

#ifdef BENCH
	double T0=DTime();
#endif
	/* compute the interpolation indexes */
	if (splineDegree & 1) 
	{
		x0 = (long)floor(x) - splineDegree / 2;
		y0 = (long)floor(y) - splineDegree / 2;
		z0 = (long)floor(z) - splineDegree / 2;
	}
	else 
	{
		x0 = (long)floor(x + 0.5) - splineDegree / 2;
		y0 = (long)floor(y + 0.5) - splineDegree / 2;
		z0 = (long)floor(z + 0.5) - splineDegree / 2;
	}

	ComputeInterpolationWeights(xWeight,yWeight,zWeight,x,y,z,x0,y0,z0);

	/* perform interpolation */
	interpolated = 0.0;
	Im3DValue zero=0.0*Ccoeff(x0,y0,z0);
	
	Im3DValue sumxyz=zero;

#ifdef BENCH
	static double T3=0;
	T3+=DTime(T0);
#endif
#ifdef NOTDEF
	Scalar ttt=0;
	for (long q = 0; q <= splineDegree; q++) 
	{
		long z=z0+q;
		Im3DValue sumxy=zero;
		for (long j = 0; j <= splineDegree; j++) 
		{
			Im3DValue sumx = zero;
			// beurk, mais sinon tres inefficace, faudrait creer un iterateur
			Im3DValue *p=Ccoeff.GetData()+(y0+j)*Width+z*Width*Height;
			for (long i = 0; i <= splineDegree; i++) 
			{
  				ttt=p[x0+i];
				if(ttt) sumx += xWeight[i] * ttt;
//  				sumx += xWeight[i] * p[x0+i];
				
			}
			sumxy += yWeight[j] * sumx;
		}
		sumxyz += zWeight[q] * sumxy;
	}
#else //NOTDEF
	for (int q = 0; q <= splineDegree; q++) 
	{
		Im3DValue sumxy=zero;
		const Im3DValue *p0=Ccoeff.GetData()+(z0+q)*Width*Height+y0*Width+x0;
		const Im3DValue *p1=p0;
		Im3DValue sumx;
		const Im3DValue *p;
		Scalar *weight;
		Scalar *yweight=yWeight;
		switch (splineDegree) 
		{

#define unrolledy_0 sumx=0;p=p1;weight=xWeight; 
#define unrolledx 	sumx +=  (Im3DValue)((*weight++) * (*p++));	
#define unrolledy_1 p1+=Width;sumxy += (Im3DValue)((*yweight++) * sumx);	

			case 2:
#define unrolledy unrolledy_0;unrolledx;unrolledx;unrolledx;unrolledy_1;
				unrolledy;unrolledy;unrolledy;
#undef unrolledy
				break;
			case 3:
#define unrolledy unrolledy_0;unrolledx;unrolledx;unrolledx;unrolledx;unrolledy_1;
				unrolledy;unrolledy;unrolledy;unrolledy;
#undef unrolledy				
				break;
			case 4:
#define unrolledy unrolledy_0;unrolledx;unrolledx;unrolledx;unrolledx;unrolledx;unrolledy_1;
				unrolledy;unrolledy;unrolledy;unrolledy;unrolledy;
#undef unrolledy				
				break;
			case 5:
#define unrolledy unrolledy_0;unrolledx;unrolledx;unrolledx;unrolledx;unrolledx;unrolledx;unrolledy_1;
				unrolledy;unrolledy;unrolledy;unrolledy;unrolledy;unrolledy;
#undef unrolledy				
				break;

#undef unrolledy_0
#undef unrolledx
#undef unrolledy_1
		}
		sumxyz += zWeight[q] * sumxy;
	}
#endif //NOTDEF
#ifdef BENCH
	static double T4=0;
	T4+=DTime(T0);

	if(!(rand()%1000))
	{
		mprintf("0-1:%4.4f 1-2:%4.4f 2-3:%4.4f 3-4:%4.4f tot:%f\n",T1/T4,(T2-T1)/T4,(T3-T2)/T4,(T4-T3)/T4,T4);
	}
#endif

	return(sumxyz);
}

#ifndef   DBL_EPSILON
#define   DBL_EPSILON              2.2204460492503131E-16
#endif // DBL_EPSILON
#ifndef   FLT_EPSILON
#define   FLT_EPSILON              ((float)1.19209290E-07)
#endif  //FLT_EPSILON


// *************************** SamplesToCoefficients ****************************
//! In-place conversion of image to coefficients
template<class Im3DValue>
void
SplineInterpolator3D<Im3DValue>::SamplesToCoefficients(ImageType &ima)
{
	int Width =ima.Width ();
	int Height=ima.Height();
	int Depth =ima.Depth ();
	if((Width<=1 || Height<=1 || Depth<=1) && boundary==BOUNDARY_MIRROR)
	{
		ThrowError("SplineInterpolator3D:: BOUNDARY_MIRROR needs image sizes in all directions to be >1");
	}

	vector<Scalar>  poles;
	long	x, y,z;


	/* recover the poles from a lookup table */
	switch (splineDegree) 
	{
	case 2:
		poles.push_back(sqrt(8.0) - 3.0);
		break;
	case 3:
		poles.push_back(sqrt(3.0) - 2.0);
		break;
	case 4:
		poles.push_back(sqrt(664.0 - sqrt(438976.0)) + sqrt(304.0) - 19.0);
		poles.push_back(sqrt(664.0 + sqrt(438976.0)) - sqrt(304.0) - 19.0);
		break;
	case 5:
		poles.push_back(sqrt(135.0 / 2.0 - sqrt(17745.0 / 4.0)) + sqrt(105.0 / 4.0)
			- 13.0 / 2.0);
		poles.push_back(sqrt(135.0 / 2.0 + sqrt(17745.0 / 4.0)) - sqrt(105.0 / 4.0)
			- 13.0 / 2.0);
		break;
	default:
		ThrowError("Invalid spline degree:%d",splineDegree);
	}

	/* convert the image samples into interpolation coefficients */
	const Scalar epsilon=(sizeof(Scalar)==sizeof(float) ? FLT_EPSILON : DBL_EPSILON);
	vector<Im3DValue> row1D;
	{
		TaskTracker<long int> tracker("SplineInterpolator3D:coefs-x",0,Depth,&z);
		/* in-place separable process, along x */
		row1D.assign(Width,0);
		for (z = 0; z < Depth; z++) 
		{
			for (y = 0; y < Height; y++) 
			{
				for(x=0;x<Width;x++){row1D[x]=ima(x,y,z);}
				ConvertToInterpolationCoefficients(row1D, poles, epsilon);
				for(x=0;x<Width;x++){ima(x,y,z)=row1D[x];}
			}
		}
	}

	/* in-place separable process, along y */
	{
		TaskTracker<long int> tracker("SplineInterpolator3D:coefs-y",0,Depth,&z);
		row1D.assign(Height,0);
		for (z = 0; z < Depth; z++) 
		{
			for (x = 0; x < Width; x++) 
			{
				for(y=0;y<Height;y++){row1D[y]=ima(x,y,z);}
				ConvertToInterpolationCoefficients(row1D, poles, epsilon);
				for(y=0;y<Height;y++){ima(x,y,z)=row1D[y];}
			}
		}
	}
	/* in-place separable process, along z */
	{
		TaskTracker<long int> tracker("SplineInterpolator3D:coefs-z",0,Width,&x);
		row1D.assign(Depth,0);
		for (x = 0L; x < Width; x++) 
		{
			for (y = 0L; y < Height; y++) 
			{
				for(z=0;z<Depth;z++){row1D[z]=ima(x,y,z);}
				ConvertToInterpolationCoefficients(row1D, poles, epsilon);
				for(z=0;z<Depth;z++){ima(x,y,z)=row1D[z];}
			}
		}
	}


} /* end SamplesToCoefficients */


template<class Im3DValue>
void
SplineInterpolator3D<Im3DValue>::ConvertToInterpolationCoefficients(
	vector<Im3DValue> &c,
	vector<Scalar> &z,
	Scalar	Tolerance	/* admissible relative error */
	)

{ /* begin ConvertToInterpolationCoefficients */
	long	DataLength=c.size();
	long	NbPoles=z.size();
	Scalar	Lambda = 1.0;
	long	n, k;

	/* special case required by mirror boundaries */
	if (DataLength == 1L) 
	{
		return;
	}
	/* compute the overall gain */
	for (k = 0L; k < NbPoles; k++) 
	{
		Lambda = Lambda * (1.0 - z[k]) * (1.0 - 1.0 / z[k]);
	}
	/* apply the gain */
	for (n = 0L; n < DataLength; n++) 
	{
		c[n] *= Lambda;
	}
	/* loop over all poles */
	for (k = 0L; k < NbPoles; k++) 
	{
		Scalar zk=z[k];
		/* causal initialization */
		c[0] = InitialCausalCoefficient(c, z[k], Tolerance);
		/* causal recursion */
		for (n = 1L; n < DataLength; n++) 
		{
			c[n] += zk * c[n - 1L];
		}
		/* anticausal initialization */
		c[DataLength - 1L] = InitialAntiCausalCoefficient(c, z[k]);
		/* anticausal recursion */
		for (n = DataLength - 2L; 0 <= n; n--) 
		{
			c[n] = zk * (c[n + 1L] - c[n]);
		}
	}
} /* end ConvertToInterpolationCoefficients */

/*--------------------------------------------------------------------------*/
template<class Im3DValue>
Scalar
SplineInterpolator3D<Im3DValue>::InitialCausalCoefficient(
	vector<Im3DValue>	&c,		    /* coefficients */
	Scalar	z,			/* actual pole */
	Scalar	Tolerance	/* admissible relative error */
	)

{
	if(boundary==BOUNDARY_ZERO){return 0*z;}
 /* begin InitialCausalCoefficient */
	long	DataLength=c.size();
	Scalar	Sum, zn, z2n, iz;
	long	n, Horizon;

	/* this initialization corresponds to mirror boundaries */
	Horizon = DataLength;
	if (Tolerance > 0.0) 
	{
		Horizon = (long)ceil(log(Tolerance) / log(fabs(z)));
	}
	if (Horizon < DataLength) 
	{
		/* accelerated loop */
		zn = z;
		Sum = c[0];
		for (n = 1L; n < Horizon; n++) 
		{
			Sum += zn * c[n];
			zn *= z;
		}
		return(Sum);
	}
	else 
	{
		/* full loop */
		zn = z;
		iz = 1.0 / z;
		z2n = pow(z, (Scalar)(DataLength - 1L));
		Sum = c[0] + z2n * c[DataLength - 1L];
		z2n *= z2n * iz;
		for (n = 1L; n <= DataLength - 2L; n++) 
		{
			Sum += (zn + z2n) * c[n];
			zn *= z;
			z2n *= iz;
		}
		return(Sum / (1.0 - zn * zn));
	}
} /* end InitialCausalCoefficient */

/*--------------------------------------------------------------------------*/
template<class Im3DValue>
Scalar
SplineInterpolator3D<Im3DValue>::InitialAntiCausalCoefficient
(
	vector<Im3DValue> &c,
	Scalar	z			/* actual pole */
	)

{
	if(boundary==BOUNDARY_ZERO){return 0*z;}
 /* begin InitialAntiCausalCoefficient */
	long	DataLength=c.size();
	/* this initialization corresponds to mirror boundaries */
	return((z / (z * z - 1.0)) * (z * c[DataLength - 2L] + c[DataLength - 1L]));
} /* end InitialAntiCausalCoefficient */





#endif // _SplineInterpolation_hxx
