/*
 * John Ashburner
 * Copyright (C) 1994-2022 Wellcome Centre for Human Neuroimaging
 */

#define TINY 5e-2

#ifdef SPM_UNSIGNED_CHAR
#define RESAMPLE resample_uchar
#define RESAMPLE_D resample_d_uchar
#define SLICE slice_uchar
#define RESAMPLE_0 resample_uchar_0
#define RESAMPLE_1 resample_uchar_1
#define RESAMPLE_D_1 resample_d_uchar_1
#define RESAMPLE_POLY resample_uchar_poly
#define RESAMPLE_D_POLY resample_d_uchar_poly
#define SLICE_0 slice_uchar_0
#define SLICE_1 slice_uchar_1
#define SLICE_POLY slice_uchar_poly
#define PLANE plane_uchar
#define GET(x) (x)
#define IMAGE_DTYPE unsigned char
#endif

#ifdef SPM_SIGNED_CHAR
#define RESAMPLE resample_schar
#define RESAMPLE_D resample_d_schar
#define SLICE slice_schar
#define RESAMPLE_0 resample_schar_0
#define RESAMPLE_1 resample_schar_1
#define RESAMPLE_D_1 resample_d_schar_1
#define RESAMPLE_POLY resample_schar_poly
#define RESAMPLE_D_POLY resample_d_schar_poly
#define SLICE_0 slice_schar_0
#define SLICE_1 slice_schar_1
#define SLICE_POLY slice_schar_poly
#define PLANE plane_schar
#define GET(x) (x)
#define IMAGE_DTYPE signed char
#endif

#ifdef SPM_SIGNED_SHORT
#ifdef SPM_BYTESWAP
#define GET(x) getshort(x)
#define RESAMPLE resample_short_s
#define RESAMPLE_D resample_d_short_s
#define SLICE slice_short_s
#define RESAMPLE_0 resample_short_s_0
#define RESAMPLE_1 resample_short_s_1
#define RESAMPLE_D_1 resample_d_short_s_1
#define RESAMPLE_POLY resample_short_s_poly
#define RESAMPLE_D_POLY resample_d_short_s_poly
#define SLICE_0 slice_short_s_0
#define SLICE_1 slice_short_s_1
#define SLICE_POLY slice_short_s_poly
#define PLANE plane_short_s
#else
#define GET(x) (x)
#define RESAMPLE resample_short
#define RESAMPLE_D resample_d_short
#define SLICE slice_short
#define RESAMPLE_0 resample_short_0
#define RESAMPLE_1 resample_short_1
#define RESAMPLE_D_1 resample_d_short_1
#define RESAMPLE_POLY resample_short_poly
#define RESAMPLE_D_POLY resample_d_short_poly
#define SLICE_0 slice_short_0
#define SLICE_1 slice_short_1
#define SLICE_POLY slice_short_poly
#define PLANE plane_short
#endif
#define IMAGE_DTYPE signed short int
#endif

#ifdef SPM_UNSIGNED_SHORT
#ifdef SPM_BYTESWAP
#define GET(x) getushort(x)
#define RESAMPLE resample_ushort_s
#define RESAMPLE_D resample_d_ushort_s
#define SLICE slice_ushort_s
#define RESAMPLE_0 resample_ushort_s_0
#define RESAMPLE_1 resample_ushort_s_1
#define RESAMPLE_D_1 resample_d_ushort_s_1
#define RESAMPLE_POLY resample_ushort_s_poly
#define RESAMPLE_D_POLY resample_d_ushort_s_poly
#define SLICE_0 slice_ushort_s_0
#define SLICE_1 slice_ushort_s_1
#define SLICE_POLY slice_ushort_s_poly
#define PLANE plane_ushort_s
#else
#define GET(x) (x)
#define RESAMPLE resample_ushort
#define RESAMPLE_D resample_d_ushort
#define SLICE slice_ushort
#define RESAMPLE_0 resample_ushort_0
#define RESAMPLE_1 resample_ushort_1
#define RESAMPLE_D_1 resample_d_ushort_1
#define RESAMPLE_POLY resample_ushort_poly
#define RESAMPLE_D_POLY resample_d_ushort_poly
#define SLICE_0 slice_ushort_0
#define SLICE_1 slice_ushort_1
#define SLICE_POLY slice_ushort_poly
#define PLANE plane_ushort
#endif
#define IMAGE_DTYPE unsigned short int
#endif

#ifdef SPM_SIGNED_INT
#ifdef SPM_BYTESWAP
#define GET(x) getint(x)
#define RESAMPLE resample_int_s
#define RESAMPLE_D resample_d_int_s
#define SLICE slice_int_s
#define RESAMPLE_0 resample_int_s_0
#define RESAMPLE_1 resample_int_s_1
#define RESAMPLE_D_1 resample_d_int_s_1
#define RESAMPLE_POLY resample_int_s_poly
#define RESAMPLE_D_POLY resample_d_int_s_poly
#define SLICE_0 slice_int_s_0
#define SLICE_1 slice_int_s_1
#define SLICE_POLY slice_int_s_poly
#define PLANE plane_int_s
#else
#define GET(x) (x)
#define RESAMPLE resample_int
#define RESAMPLE_D resample_d_int
#define SLICE slice_int
#define RESAMPLE_0 resample_int_0
#define RESAMPLE_1 resample_int_1
#define RESAMPLE_D_1 resample_d_int_1
#define RESAMPLE_POLY resample_int_poly
#define RESAMPLE_D_POLY resample_d_int_poly
#define SLICE_0 slice_int_0
#define SLICE_1 slice_int_1
#define SLICE_POLY slice_int_poly
#define PLANE plane_int
#endif
#define IMAGE_DTYPE signed int
#endif

#ifdef SPM_UNSIGNED_INT
#ifdef SPM_BYTESWAP
#define GET(x) getuint(x)
#define RESAMPLE resample_uint_s
#define RESAMPLE_D resample_d_uint_s
#define SLICE slice_uint_s
#define RESAMPLE_0 resample_uint_s_0
#define RESAMPLE_1 resample_uint_s_1
#define RESAMPLE_D_1 resample_d_uint_s_1
#define RESAMPLE_POLY resample_uint_s_poly
#define RESAMPLE_D_POLY resample_d_uint_s_poly
#define SLICE_0 slice_uint_s_0
#define SLICE_1 slice_uint_s_1
#define SLICE_POLY slice_uint_s_poly
#define PLANE plane_uint_s
#else
#define GET(x) (x)
#define RESAMPLE resample_uint
#define RESAMPLE_D resample_d_uint
#define SLICE slice_uint
#define RESAMPLE_0 resample_uint_0
#define RESAMPLE_1 resample_uint_1
#define RESAMPLE_D_1 resample_d_uint_1
#define RESAMPLE_POLY resample_uint_poly
#define RESAMPLE_D_POLY resample_d_uint_poly
#define SLICE_0 slice_uint_0
#define SLICE_1 slice_uint_1
#define SLICE_POLY slice_uint_poly
#define PLANE plane_uint
#endif
#define IMAGE_DTYPE unsigned int
#endif

#ifdef SPM_UNSIGNED_LONG_LONG
#ifdef SPM_BYTESWAP
#define GET(x) getuint64(x)
#define RESAMPLE resample_uint64_s
#define RESAMPLE_D resample_d_uint64_s
#define SLICE slice_uint64_s
#define RESAMPLE_0 resample_uint64_s_0
#define RESAMPLE_1 resample_uint64_s_1
#define RESAMPLE_D_1 resample_d_uint64_s_1
#define RESAMPLE_POLY resample_uint64_s_poly
#define RESAMPLE_D_POLY resample_d_uint64_s_poly
#define SLICE_0 slice_uint64_s_0
#define SLICE_1 slice_uint64_s_1
#define SLICE_POLY slice_uint64_s_poly
#define PLANE plane_uint64_s
#else
#define GET(x) (x)
#define RESAMPLE resample_uint64
#define RESAMPLE_D resample_d_uint64
#define SLICE slice_uint64
#define RESAMPLE_0 resample_uint64_0
#define RESAMPLE_1 resample_uint64_1
#define RESAMPLE_D_1 resample_d_uint64_1
#define RESAMPLE_POLY resample_uint64_poly
#define RESAMPLE_D_POLY resample_d_uint64_poly
#define SLICE_0 slice_uint64_0
#define SLICE_1 slice_uint64_1
#define SLICE_POLY slice_uint64_poly
#define PLANE plane_uint64
#endif
#define IMAGE_DTYPE unsigned long long int
#endif

#ifdef SPM_SIGNED_LONG_LONG
#ifdef SPM_BYTESWAP
#define GET(x) getint64(x)
#define RESAMPLE resample_int64_s
#define RESAMPLE_D resample_d_int64_s
#define SLICE slice_int64_s
#define RESAMPLE_0 resample_int64_s_0
#define RESAMPLE_1 resample_int64_s_1
#define RESAMPLE_D_1 resample_d_int64_s_1
#define RESAMPLE_POLY resample_int64_s_poly
#define RESAMPLE_D_POLY resample_d_int64_s_poly
#define SLICE_0 slice_int64_s_0
#define SLICE_1 slice_int64_s_1
#define SLICE_POLY slice_int64_s_poly
#define PLANE plane_int64_s
#else
#define GET(x) (x)
#define RESAMPLE resample_int64
#define RESAMPLE_D resample_d_int64
#define SLICE slice_int64
#define RESAMPLE_0 resample_int64_0
#define RESAMPLE_1 resample_int64_1
#define RESAMPLE_D_1 resample_d_int64_1
#define RESAMPLE_POLY resample_int64_poly
#define RESAMPLE_D_POLY resample_d_int64_poly
#define SLICE_0 slice_int64_0
#define SLICE_1 slice_int64_1
#define SLICE_POLY slice_int64_poly
#define PLANE plane_int64
#endif
#define IMAGE_DTYPE signed long long int
#endif

#ifdef SPM_FLOAT
#ifdef SPM_BYTESWAP
#define GET(x) getfloat(x)
#define RESAMPLE resample_float_s
#define RESAMPLE_D resample_d_float_s
#define SLICE slice_float_s
#define RESAMPLE_0 resample_float_s_0
#define RESAMPLE_1 resample_float_s_1
#define RESAMPLE_D_1 resample_d_float_s_1
#define RESAMPLE_POLY resample_float_s_poly
#define RESAMPLE_D_POLY resample_d_float_s_poly
#define SLICE_0 slice_float_s_0
#define SLICE_1 slice_float_s_1
#define SLICE_POLY slice_float_s_poly
#define PLANE plane_float_s
#else
#define GET(x) (x)
#define RESAMPLE resample_float
#define RESAMPLE_D resample_d_float
#define SLICE slice_float
#define RESAMPLE_0 resample_float_0
#define RESAMPLE_1 resample_float_1
#define RESAMPLE_D_1 resample_d_float_1
#define RESAMPLE_POLY resample_float_poly
#define RESAMPLE_D_POLY resample_d_float_poly
#define SLICE_0 slice_float_0
#define SLICE_1 slice_float_1
#define SLICE_POLY slice_float_poly
#define PLANE plane_float
#endif
#define IMAGE_DTYPE float
#endif

#ifdef SPM_DOUBLE
#ifdef SPM_BYTESWAP
#define GET(x) getdouble(x)
#define RESAMPLE resample_double_s
#define RESAMPLE_D resample_d_double_s
#define SLICE slice_double_s
#define RESAMPLE_0 resample_double_s_0
#define RESAMPLE_1 resample_double_s_1
#define RESAMPLE_D_1 resample_d_double_s_1
#define RESAMPLE_POLY resample_double_s_poly
#define RESAMPLE_D_POLY resample_d_double_s_poly
#define SLICE_0 slice_double_s_0
#define SLICE_1 slice_double_s_1
#define SLICE_POLY slice_double_s_poly
#define PLANE plane_double_s
#else
#define GET(x) (x)
#define RESAMPLE resample_double
#define RESAMPLE_D resample_d_double
#define SLICE slice_double
#define RESAMPLE_0 resample_double_0
#define RESAMPLE_1 resample_double_1
#define RESAMPLE_D_1 resample_d_double_1
#define RESAMPLE_POLY resample_double_poly
#define RESAMPLE_D_POLY resample_d_double_poly
#define SLICE_0 slice_double_0
#define SLICE_1 slice_double_1
#define SLICE_POLY slice_double_poly
#define PLANE plane_double
#endif
#define IMAGE_DTYPE double
#endif

#include <math.h>
#include <stdlib.h>
#define RINT(A) floor((A)+0.5)
#include "spm_make_lookup.h"
#include "spm_getdata.h"

static void (*make_lookup)() = make_lookup_poly, (*make_lookup_grad)() = make_lookup_poly_grad;

/* Zero order hold resampling - nearest neighbour */
void RESAMPLE_0(m,vol,out,x,y,z,xdim,ydim,zdim,background, scale,offset)
int m, xdim,ydim,zdim;
double out[], x[], y[], z[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    int i;
    for (i=0; i<m; i++)
    {
        int xcoord, ycoord, zcoord;
        xcoord = floor(x[i]-0.5);
        ycoord = floor(y[i]-0.5);
        zcoord = floor(z[i]-0.5);
        if (xcoord>=0 && xcoord<xdim && ycoord>=0 &&
            ycoord<ydim && zcoord>=0 && zcoord<zdim)
            out[i] = scale[zcoord]*GET(vol[zcoord][xcoord  + xdim*ycoord])+offset[zcoord];
        else out[i] = background;
    }
}


/* First order hold resampling - trilinear interpolation */
void RESAMPLE_1(m,vol,out,x,y,z,xdim,ydim,zdim,background, scale,offset)
int m, xdim,ydim,zdim;
double out[], x[], y[], z[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    int i;
    for (i=0; i<m; i++)
    {
        double xi,yi,zi;
        xi=x[i]-1.0;
        yi=y[i]-1.0;
        zi=z[i]-1.0;

        if (    zi>=-TINY && zi<zdim+TINY-1 &&
            yi>=-TINY && yi<ydim+TINY-1 &&
            xi>=-TINY && xi<xdim+TINY-1)
        {
            double k111,k112,k121,k122,k211,k212,k221,k222;
            double dx1, dx2, dy1, dy2, dz1, dz2;
            int off1, off2, offx, offy, offz, xcoord, ycoord, zcoord;

            xcoord = (int)floor(xi); dx1=xi-xcoord; dx2=1.0-dx1;
            ycoord = (int)floor(yi); dy1=yi-ycoord; dy2=1.0-dy1;
            zcoord = (int)floor(zi); dz1=zi-zcoord; dz2=1.0-dz1;

            xcoord = (xcoord < 0) ? ((offx=0),0) : ((xcoord>=xdim-1) ? ((offx=0),xdim-1) : ((offx=1   ),xcoord));
            ycoord = (ycoord < 0) ? ((offy=0),0) : ((ycoord>=ydim-1) ? ((offy=0),ydim-1) : ((offy=xdim),ycoord));
            zcoord = (zcoord < 0) ? ((offz=0),0) : ((zcoord>=zdim-1) ? ((offz=0),zdim-1) : ((offz=1   ),zcoord));

            off1 = xcoord  + xdim*ycoord;
            off2 = off1+offy;
            k222 = GET(vol[zcoord     ][off1]); k122 = GET(vol[zcoord     ][off1+offx]);
            k212 = GET(vol[zcoord     ][off2]); k112 = GET(vol[zcoord     ][off2+offx]);
            k221 = GET(vol[zcoord+offz][off1]); k121 = GET(vol[zcoord+offz][off1+offx]);
            k211 = GET(vol[zcoord+offz][off2]); k111 = GET(vol[zcoord+offz][off2+offx]);

            /* resampled pixel value (trilinear interpolation) */
            out[i] =  (((k222*dx2 + k122*dx1)*dy2 + (k212*dx2 + k112*dx1)*dy1)*scale[zcoord     ] + offset[zcoord     ])*dz2
                + (((k221*dx2 + k121*dx1)*dy2 + (k211*dx2 + k111*dx1)*dy1)*scale[zcoord+offz] + offset[zcoord+offz])*dz1;
        }
        else out[i] = background;

    }
}

/* First order hold resampling - trilinear interpolation */
void RESAMPLE_D_1(m,vol,out,gradx,grady,gradz,x,y,z,xdim,ydim,zdim,background, scale,offset)
int m, xdim,ydim,zdim;
double out[],gradx[],grady[],gradz[], x[], y[], z[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    int i;
    for (i=0; i<m; i++)
    {
        double xi,yi,zi;
        xi=x[i]-1.0;
        yi=y[i]-1.0;
        zi=z[i]-1.0;
        if (zi>=-TINY && zi<zdim+TINY-1 &&
            yi>=-TINY && yi<ydim+TINY-1 &&
            xi>=-TINY && xi<xdim+TINY-1)
        {
            double k111,k112,k121,k122,k211,k212,k221,k222;
            double dx1, dx2, dy1, dy2, dz1, dz2;
            int off1, off2, offx, offy, offz, xcoord, ycoord, zcoord;

            xcoord = (int)floor(xi); dx1=xi-xcoord; dx2=1.0-dx1;
            ycoord = (int)floor(yi); dy1=yi-ycoord; dy2=1.0-dy1;
            zcoord = (int)floor(zi); dz1=zi-zcoord; dz2=1.0-dz1;

            xcoord = (xcoord < 0) ? ((offx=0),0) : ((xcoord>=xdim-1) ? ((offx=0),xdim-1) : ((offx=1   ),xcoord));
            ycoord = (ycoord < 0) ? ((offy=0),0) : ((ycoord>=ydim-1) ? ((offy=0),ydim-1) : ((offy=xdim),ycoord));
            zcoord = (zcoord < 0) ? ((offz=0),0) : ((zcoord>=zdim-1) ? ((offz=0),zdim-1) : ((offz=1   ),zcoord));

            off1 = xcoord  + xdim*ycoord;
            off2 = off1+offy;
            k222 = GET(vol[zcoord     ][off1]); k122 = GET(vol[zcoord     ][off1+offx]);
            k212 = GET(vol[zcoord     ][off2]); k112 = GET(vol[zcoord     ][off2+offx]);
            k221 = GET(vol[zcoord+offz][off1]); k121 = GET(vol[zcoord+offz][off1+offx]);
            k211 = GET(vol[zcoord+offz][off2]); k111 = GET(vol[zcoord+offz][off2+offx]);

            /* resampled pixel value (trilinear interpolation) and gradients
               old code:
            gradx[i] = scale*(((k111     - k211    )*dy1 + (k121     - k221    )*dy2)*dz1
                            + ((k112     - k212    )*dy1 + (k122     - k222    )*dy2)*dz2);
            grady[i] = scale*(((k111*dx1 + k211*dx2)     - (k121*dx1 + k221*dx2)    )*dz1
                            + ((k112*dx1 + k212*dx2)     - (k122*dx1 + k222*dx2)    )*dz2);
            gradz[i] = scale*(((k111*dx1 + k211*dx2)*dy1 + (k121*dx1 + k221*dx2)*dy2)
                            - ((k112*dx1 + k212*dx2)*dy1 + (k122*dx1 + k222*dx2)*dy2)    );

            out[i]   = scale*(((k111*dx1 + k211*dx2)*dy1 + (k121*dx1 + k221*dx2)*dy2)*dz1
                            + ((k112*dx1 + k212*dx2)*dy1 + (k122*dx1 + k222*dx2)*dy2)*dz2) + offset; */

            gradx[i] = (((k111 - k211)*dy1 + (k121 - k221)*dy2)*scale[zcoord+offz])*dz1
                     + (((k112 - k212)*dy1 + (k122 - k222)*dy2)*scale[zcoord     ])*dz2;

            k111 = (k111*dx1 + k211*dx2)*scale[zcoord+offz]+offset[zcoord+offz];
            k121 = (k121*dx1 + k221*dx2)*scale[zcoord+offz]+offset[zcoord+offz];
            k112 = (k112*dx1 + k212*dx2)*scale[zcoord     ]+offset[zcoord     ];
            k122 = (k122*dx1 + k222*dx2)*scale[zcoord     ]+offset[zcoord     ];

            grady[i] = (k111 - k121)*dz1 + (k112 - k122)*dz2;

            k111 = k111*dy1 + k121*dy2;
            k112 = k112*dy1 + k122*dy2;

            gradz[i] = k111 - k112;
            out[i]   = k111*dz1 + k112*dz2;
        }
        else
        {
            out[i]   = background;
            gradx[i] = 0.0;
            grady[i] = 0.0;
            gradz[i] = 0.0;
        }
    }
}



/* Sinc resampling */
void RESAMPLE_POLY(m,vol,out,x,y,z,xdim,ydim,zdim, q,background, scale,offset)
int m, xdim,ydim,zdim, q;
double out[], x[], y[], z[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    int i;
    int dx1, dy1, dz1;
    static double tablex[255], tabley[255], tablez[255];

    for (i=0; i<m; i++)
    {
        if (z[i]>=1-TINY && z[i]<=zdim+TINY &&
            y[i]>=1-TINY && y[i]<=ydim+TINY &&
            x[i]>=1-TINY && x[i]<=xdim+TINY)
        {
            double dat=0.0, *tp1, *tp1end, *tp2end, *tp3end;

            make_lookup(x[i], q, xdim, &dx1, tablex, &tp3end);
            make_lookup(y[i], q, ydim, &dy1, tabley, &tp2end);
            make_lookup(z[i], q, zdim, &dz1, tablez, &tp1end);

            tp1 = tablez;
            dy1 *= xdim;

            while(tp1 <= tp1end)
            {
                IMAGE_DTYPE *dp2 = &vol[dz1][dy1];
                double dat2 = 0.0,
                *tp2 = tabley;
                while (tp2 <= tp2end)
                {
                    register double dat3 = 0.0, *tp3 = tablex;
                    register IMAGE_DTYPE *dp3 = dp2 + dx1;
                    while(tp3 <= tp3end)
                        dat3 += GET(*(dp3++)) * *(tp3++);
                    dat2 += dat3 * *(tp2++);
                    dp2  += xdim;
                }
                dat += (dat2*scale[dz1]+offset[dz1]) * *(tp1++);
                dz1 ++;
            }
            out[i] = dat;
        }
        else out[i] = background;
    }
}

/* Sinc resampling */
void RESAMPLE_D_POLY(m,vol,out,gradx,grady,gradz,x,y,z,xdim,ydim,zdim, q,background, scale,offset)
int m, xdim,ydim,zdim, q;
double out[],gradx[],grady[],gradz[], x[], y[], z[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    int i;
    int dx1, dy1, dz1;
    static double  tablex[255],  tabley[255],  tablez[255];
    static double dtablex[255], dtabley[255], dtablez[255];

    for (i=0; i<m; i++)
    {
        if (z[i]>=1-TINY && z[i]<=zdim+TINY &&
            y[i]>=1-TINY && y[i]<=ydim+TINY &&
            x[i]>=1-TINY && x[i]<=xdim+TINY)
        {
            double dat=0.0, datx = 0.0, daty = 0.0, datz = 0.0,
                  *tp1, *tp1end, *tp2end, *tp3end, *dp1;
            make_lookup_grad(x[i], q, xdim, &dx1, tablex, dtablex, &tp3end);
            make_lookup_grad(y[i], q, ydim, &dy1, tabley, dtabley, &tp2end);
            make_lookup_grad(z[i], q, zdim, &dz1, tablez, dtablez, &tp1end);
            tp1 =  tablez;
            dp1 = dtablez;
            dy1 *= xdim;
            while(tp1 <= tp1end)
            {
                IMAGE_DTYPE *d2 = &vol[dz1][dy1];
                double dat2  = 0.0, *tp2 =  tabley;
                double dat2x = 0.0, *dp2 = dtabley, dat2y = 0.0;
                while (tp2 <= tp2end)
                {
                    register IMAGE_DTYPE *d3 = d2 + dx1;
                    register double dat3  = 0.0, *tp3 =  tablex;
                    register double dat3x = 0.0, *dp3 = dtablex;
                    while(tp3 <= tp3end)
                    {
                        dat3x += GET(*(d3  )) * *(dp3++);
                        dat3  += GET(*(d3++)) * *(tp3++);
                    }
                    dat2x += dat3x * *(tp2  );
                    dat2  += dat3  * *(tp2++);
                    dat2y += dat3  * *(dp2++);
                    d2 += xdim;
                }

                datx += (dat2x*scale[dz1]) * *(tp1  );
                daty += (dat2y*scale[dz1]) * *(tp1  );
                dat2  = dat2*scale[dz1]+offset[dz1];
                dat  += dat2  * *(tp1++);
                datz += dat2  * *(dp1++);
                dz1  ++;
            }
            out[i]   = dat;
            gradx[i] = datx;
            grady[i] = daty;
            gradz[i] = datz;
        }
        else
        {
            out[i]   = background;
            gradx[i] = 0.0;
            grady[i] = 0.0;
            gradz[i] = 0.0;
        }
    }
}


/* Zero order hold resampling - nearest neighbour */
int SLICE_0(mat, image, xdim1, ydim1, vol, xdim2, ydim2, zdim2, background, scale,offset)
double  mat[16], background, scale[],offset[];
double image[];
IMAGE_DTYPE *vol[];
int xdim1, ydim1, xdim2, ydim2, zdim2;
{
    double y, x2, y2, z2, s2, dx3=mat[0], dy3=mat[1], dz3=mat[2], ds3=mat[3];
    int t = 0;

    x2 = mat[12] + 0*mat[8];
    y2 = mat[13] + 0*mat[9];
    z2 = mat[14] + 0*mat[10];
    s2 = mat[15] + 0*mat[11];

    for(y=1; y<=ydim1; y++)
    {
        double x;
        double x3 = x2 + y*mat[4];
        double y3 = y2 + y*mat[5];
        double z3 = z2 + y*mat[6];
        double s3 = s2 + y*mat[7];

        for(x=1; x<=xdim1; x++)
        {
            int ix4, iy4, iz4;
            s3 += ds3;
            if (s3 == 0.0) return(-1);
            ix4 = floor(((x3 += dx3)/s3)-0.5);
            iy4 = floor(((y3 += dy3)/s3)-0.5);
            iz4 = floor(((z3 += dz3)/s3)-0.5);
            if (iz4>=0 && iz4<zdim2 && iy4>=0 && iy4<ydim2 && ix4>=0 && ix4<xdim2)
            {
                image[t] = scale[iz4]*(double)GET(vol[iz4][ix4 + xdim2*iy4]) + offset[iz4];
            }
            else image[t] = background;
            t++;
        }
    }
    return(0);
}

#define TINY 5e-2

/* First order hold resampling - trilinear interpolation */
int SLICE_1(mat, image, xdim1, ydim1, vol, xdim2, ydim2, zdim2, background, scale,offset)
double  mat[16], background, scale[],offset[];
double image[];
IMAGE_DTYPE *vol[];
int xdim1, ydim1, xdim2, ydim2, zdim2;
{
    double y, x2, y2, z2, s2, dx3=mat[0], dy3=mat[1], dz3=mat[2], ds3=mat[3];
    int t = 0;

    x2 = mat[12] + 0*mat[8];
    y2 = mat[13] + 0*mat[9];
    z2 = mat[14] + 0*mat[10];
    s2 = mat[15] + 0*mat[11];

    for(y=1; y<=ydim1; y++)
    {
        double x;
        double x3 = x2 + y*mat[4];
        double y3 = y2 + y*mat[5];
        double z3 = z2 + y*mat[6];
        double s3 = s2 + y*mat[7];
        for(x=1; x<=xdim1; x++)
        {
            double x4,y4,z4;
            s3 += ds3;
            if (s3 == 0.0) return(-1);
            x4=(x3 += dx3)/s3-1.0;
            y4=(y3 += dy3)/s3-1.0;
            z4=(z3 += dz3)/s3-1.0;

            if (    z4>=-TINY && z4<zdim2+TINY-1 &&
                y4>=-TINY && y4<ydim2+TINY-1 &&
                x4>=-TINY && x4<xdim2+TINY-1)
            {
                double k111,k112,k121,k122,k211,k212,k221,k222;
                double dx1, dx2, dy1, dy2, dz1, dz2;
                int off1, off2, offx, offy, offz, ix4, iy4, iz4;

                ix4 = floor(x4); dx1=x4-ix4; dx2=1.0-dx1;
                iy4 = floor(y4); dy1=y4-iy4; dy2=1.0-dy1;
                iz4 = floor(z4); dz1=z4-iz4; dz2=1.0-dz1;

                ix4 = (ix4 < 0) ? ((offx=0),0) : ((ix4>=xdim2-1) ? ((offx=0),xdim2-1) : ((offx=1    ),ix4));
                iy4 = (iy4 < 0) ? ((offy=0),0) : ((iy4>=ydim2-1) ? ((offy=0),ydim2-1) : ((offy=xdim2),iy4));
                iz4 = (iz4 < 0) ? ((offz=0),0) : ((iz4>=zdim2-1) ? ((offz=0),zdim2-1) : ((offz=1    ),iz4));

                off1 = ix4  + xdim2*iy4;
                off2 = off1+offy;
                k222 = GET(vol[iz4     ][off1]); k122 = GET(vol[iz4     ][off1+offx]);
                k212 = GET(vol[iz4     ][off2]); k112 = GET(vol[iz4     ][off2+offx]);
                k221 = GET(vol[iz4+offz][off1]); k121 = GET(vol[iz4+offz][off1+offx]);
                k211 = GET(vol[iz4+offz][off2]); k111 = GET(vol[iz4+offz][off2+offx]);

                /* resampled pixel value (trilinear interpolation) */
                image[t] = (((k222*dx2 + k122*dx1)*dy2 + (k212*dx2 + k112*dx1)*dy1)*scale[iz4     ] + offset[iz4     ])*dz2
                     + (((k221*dx2 + k121*dx1)*dy2 + (k211*dx2 + k111*dx1)*dy1)*scale[iz4+offz] + offset[iz4+offz])*dz1;

            }
            else image[t] = background; 
            t++;
        }
    }
    return(0);
}


/* Sinc resampling */
int SLICE_POLY(mat, image, xdim1, ydim1, vol, xdim2, ydim2, zdim2, q, background, scale,offset)
int ydim1,xdim1, xdim2,ydim2,zdim2, q;
double image[], mat[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    int dx1, dy1, dz1;
    static double tablex[255], tabley[255], tablez[255];
    double y, x2, y2, z2, s2, dx3=mat[0], dy3=mat[1], dz3=mat[2], ds3=mat[3];

    x2 = mat[12] + 0*mat[8];
    y2 = mat[13] + 0*mat[9];
    z2 = mat[14] + 0*mat[10];
    s2 = mat[15] + 0*mat[11];

    for(y=1; y<=ydim1; y++)
    {
        double x;
        double x3 = x2 + y*mat[4];
        double y3 = y2 + y*mat[5];
        double z3 = z2 + y*mat[6];
        double s3 = s2 + y*mat[7];

        for(x=1; x<=xdim1; x++)
        {
            double x4,y4,z4;
            s3 += ds3;
            if (s3 == 0.0) return(-1);
            x4=(x3 += dx3)/s3;
            y4=(y3 += dy3)/s3;
            z4=(z3 += dz3)/s3;

            if (z4>=1-TINY && z4<=zdim2+TINY &&
                y4>=1-TINY && y4<=ydim2+TINY &&
                x4>=1-TINY && x4<=xdim2+TINY)
            {
                double dat=0.0, *tp1, *tp1end, *tp2end, *tp3end;

                make_lookup(x4, q, xdim2, &dx1, tablex, &tp3end);
                make_lookup(y4, q, ydim2, &dy1, tabley, &tp2end);
                make_lookup(z4, q, zdim2, &dz1, tablez, &tp1end);

                tp1 = tablez;
                dy1 *= xdim2;

                while(tp1 <= tp1end)
                {
                    IMAGE_DTYPE *dp2 = &vol[dz1][dy1];
                    double dat2 = 0.0,
                    *tp2 = tabley;
                    while (tp2 <= tp2end)
                    {
                        register double dat3 = 0.0, *tp3 = tablex;
                        register IMAGE_DTYPE *dp3 = dp2 + dx1;
                        while(tp3 <= tp3end)
                            dat3 += GET(*(dp3++)) * *(tp3++);
                        dat2 += dat3 * *(tp2++);
                        dp2 += xdim2;
                    }
                    dat += (dat2*scale[dz1]+offset[dz1]) * *(tp1++);
                    dz1 ++;
                }
                *(image++) = dat;
            }
            else *(image++) = background;
        }
    }
    return(0);
}

/* simple extraction of transverse plane */
void PLANE(p,image,vol,xdim,ydim,scale,offset)
int p, xdim,ydim;
double image[], scale[],offset[];
IMAGE_DTYPE *vol[];
{
    int n = xdim*ydim, i;
    IMAGE_DTYPE *ptr = vol[p-1];
    for(i=0; i<n; i++)
        image[i] = GET(ptr[i])*scale[p-1] + offset[p-1];
}

/* Extract a slice through the image */
int SLICE(mat, image, xdim1,ydim1, vol, xdim2,ydim2,zdim2, hold,background, scale,offset)
int ydim1,xdim1, xdim2,ydim2,zdim2, hold;
double image[], mat[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    double t = 1e-10;
    /* attempt a nice easy transverse plane first */
    if (mat[0+0*4] > 1-t && mat[0+0*4] < 1+t &&
        mat[0+1*4] > -t  && mat[0+1*4] < t   &&
        mat[0+2*4] > -t  && mat[0+2*4] < t   &&
        mat[0+3*4] > -t  && mat[0+3*4] < t   &&
        mat[1+0*4] > -t  && mat[1+0*4] < t   &&
        mat[1+1*4] > 1-t && mat[1+1*4] < 1+t &&
        mat[1+2*4] > -t  && mat[1+2*4] < t   &&
        mat[1+3*4] > -t  && mat[1+3*4] < t   &&
        mat[2+0*4] > -t  && mat[2+0*4] < t   &&
        mat[2+1*4] > -t  && mat[2+1*4] < t   &&
        mat[2+2*4] > 1-t && mat[2+2*4] < 1+t &&
        fabs(RINT(mat[2+3*4])-mat[2+3*4])<t  &&
        mat[3+0*4] > -t  && mat[3+0*4] < t   &&
        mat[3+1*4] > -t  && mat[3+1*4] < t   &&
        mat[3+2*4] > -t  && mat[3+2*4] < t   &&
        mat[3+3*4] > 1-t && mat[3+3*4] < 1+t &&
        xdim1 == xdim2 && ydim1 == ydim2 && mat[2+3*4]>=1.0 && mat[2+3*4]<=zdim2)
    {
        int p;
        p = RINT(mat[2+3*4]);
        PLANE(p,image,vol,xdim2,ydim2,scale,offset);
        return(0);
    }
    else
    {
        if (hold<0)
        {
            hold=abs(hold);
            make_lookup = make_lookup_sinc;
            return(SLICE_POLY(mat, image, xdim1, ydim1, vol, xdim2, ydim2, zdim2, hold+1, background, scale,offset));
        }
        else
        {
            make_lookup = make_lookup_poly;
        }
        if (hold == 0)
            return(SLICE_0(mat, image, xdim1, ydim1, vol, xdim2, ydim2, zdim2, background, scale,offset));
        if (hold == 1)
            return(SLICE_1(mat, image, xdim1, ydim1, vol, xdim2, ydim2, zdim2, background, scale,offset));
        else
            return(SLICE_POLY(mat, image, xdim1, ydim1, vol, xdim2, ydim2, zdim2, hold+1, background, scale,offset));
    }
}

/* Resample image */
void RESAMPLE(m,vol,out,x,y,z,xdim,ydim,zdim, hold, background, scale,offset)
int m, xdim,ydim,zdim, hold;
double out[], x[], y[], z[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    if (hold<0)
    {
        hold=abs(hold);
        make_lookup = make_lookup_sinc;
        RESAMPLE_POLY(m,vol,out,x,y,z,xdim,ydim,zdim, hold+1, background, scale,offset);
    }
    else
    {
        make_lookup = make_lookup_poly;
        if (hold == 0)
            RESAMPLE_0(m,vol,out,x,y,z,xdim,ydim,zdim, background, scale,offset);
        else if (hold == 1)
            RESAMPLE_1(m,vol,out,x,y,z,xdim,ydim,zdim, background, scale,offset);
        else
            RESAMPLE_POLY(m,vol,out,x,y,z,xdim,ydim,zdim, hold+1, background, scale,offset);
    }
}

/* Resample image and derivatives */
void RESAMPLE_D(m,vol,out,gradx,grady,gradz,x,y,z,xdim,ydim,zdim, hold, background, scale,offset)
int m, xdim,ydim,zdim, hold;
double out[],gradx[],grady[],gradz[], x[], y[], z[], background, scale[],offset[];
IMAGE_DTYPE *vol[];
{
    if (hold<0)
    {
        hold=abs(hold);
        make_lookup_grad = make_lookup_sinc_grad;
        RESAMPLE_D_POLY(m,vol,out,gradx,grady,gradz,x,y,z,xdim,ydim,zdim, hold+1, background, scale,offset);
    }
    else
    {
        make_lookup_grad = make_lookup_poly_grad;
        if (hold == 1)
            RESAMPLE_D_1(m,vol,out,gradx,grady,gradz,x,y,z,xdim,ydim,zdim, background, scale,offset);
        else
            RESAMPLE_D_POLY(m,vol,out,gradx,grady,gradz,x,y,z,xdim,ydim,zdim, hold+1, background, scale,offset);
    }
}
