//  
//  splinterpolator.h
//
// Jesper Andersson, FMRIB Image Analysis Group
//
// Copyright (C) 2008 University of Oxford 
//
//     CCOPYRIGHT
//

#ifndef splinterpolator_h
#define splinterpolator_h

#include <vector>
#include <string>
#include <cmath>
#include "newmat.h"
#include "miscmaths/miscmaths.h"

namespace SPLINTERPOLATOR {

enum ExtrapolationType {Zeros, Constant, Mirror, Periodic};

class SplinterpolatorException: public std::exception
{
private:
  std::string m_msg;
public:
  SplinterpolatorException(const std::string& msg) throw(): m_msg(msg) {}

  virtual const char *what() const throw() {
    return string("Splinterpolator::" + m_msg).c_str();
  }

  ~SplinterpolatorException() throw() {}
};

//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//
// Class Splinterpolator:
//
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

template<class T>
class Splinterpolator
{
public:
  // Constructors
  Splinterpolator() : _valid(false), _own_coef(false), _coef(0), _cptr(0), _ndim(0) {}
  Splinterpolator(const T *data, const std::vector<unsigned int>& dim, const std::vector<ExtrapolationType>& et, unsigned int order=3, bool copy_low_order=true, double prec=1e-8) : _valid(false), _own_coef(false), _coef(0), _cptr(0), _ndim(0)
  {
    common_construction(data,dim,order,prec,et,copy_low_order);
  }
  Splinterpolator(const T *data, const std::vector<unsigned int>& dim, ExtrapolationType et=Zeros, unsigned int order=3, bool copy_low_order=true, double prec=1e-8) : _valid(false), _own_coef(false), _coef(0), _cptr(0), _ndim(0)
  {
    std::vector<ExtrapolationType>   ett(dim.size(),et);
    common_construction(data,dim,order,prec,ett,copy_low_order);
  }
  // Copy construction. May be removed in future
  Splinterpolator(const Splinterpolator<T>& src) : _valid(false), _own_coef(false), _coef(0), _cptr(0), _ndim(0) { assign(src); }

  // Destructor
  ~Splinterpolator() { if(_own_coef) delete [] _coef; }

  // Assignment. May be removed in future
  Splinterpolator& operator=(const Splinterpolator& src) { if(_own_coef) delete [] _coef; assign(src); return(*this); }

  // Set new data in Splinterpolator.
  void Set(const T *data, const std::vector<unsigned int>& dim, const std::vector<ExtrapolationType>& et, unsigned int order=3, bool copy_low_order=true, double prec=1e-8)
  {
    if (_own_coef) delete [] _coef;
    common_construction(data,dim,order,prec,et,copy_low_order);
  }
  void Set(const T *data, const std::vector<unsigned int>& dim, ExtrapolationType et, unsigned int order=3, bool copy_low_order=true, double prec=1e-8)
  {
    std::vector<ExtrapolationType>  vet(dim.size(),Zeros);
    Set(data,dim,vet,order,copy_low_order,prec);
  }

  // Return interpolated value
  T operator()(const std::vector<float>&  coord) const;
  T operator()(double x, double y=0, double z=0, double t=0) const
  {
   if (!_valid) throw SplinterpolatorException("operator(): Cannot interpolate un-initialized object");
   if (_ndim>4 || (t && _ndim<4) || (z && _ndim<3) || (y && _ndim<2)) throw SplinterpolatorException("operator(): input has wrong dimensionality");
    double coord[5] = {x,y,z,t,0.0};
    return(static_cast<T>(value_at(coord)));
  }

  // Return interpolated value along with first derivative in one direction (useful for distortion correction)
  T operator()(const std::vector<float>& coord, unsigned int dd, T *dval) const;
  T operator()(double x, double y, double z, unsigned int dd, T *dval) const; 
  T operator()(double x, double y, unsigned int dd, T *dval) const { return((*this)(x,y,0.0,dd,dval)); }
  T operator()(double x, T *dval) const { return((*this)(x,0.0,0.0,0,dval)); }

  // Return interpolated value along with selected derivatives
  T ValAndDerivs(const std::vector<float>& coord, const std::vector<unsigned int>& deriv, std::vector<T>& rderiv) const;
  T ValAndDerivs(const std::vector<float>& coord, std::vector<T>& rderiv) const
  {
    std::vector<unsigned int>   deriv(_ndim,1);
    return(ValAndDerivs(coord,deriv,rderiv));
  }
  T ValAndDerivs(double x, double y, double z, std::vector<T>& rderiv) const;

  // Return continous derivative at voxel centres (only works for order<1)
  T Deriv(const std::vector<unsigned int>& indx, unsigned int ddir) const;
  T Deriv1(const std::vector<unsigned int>& indx) const {return(Deriv(indx,0));}
  T Deriv2(const std::vector<unsigned int>& indx) const {return(Deriv(indx,1));}
  T Deriv3(const std::vector<unsigned int>& indx) const {return(Deriv(indx,2));}
  T Deriv4(const std::vector<unsigned int>& indx) const {return(Deriv(indx,3));}
  T Deriv5(const std::vector<unsigned int>& indx) const {return(Deriv(indx,4));}
  T DerivXYZ(unsigned int i, unsigned int j, unsigned int k, unsigned int dd) const;
  T DerivX(unsigned int i, unsigned int j, unsigned int k) const {return(DerivXYZ(i,j,k,0));}
  T DerivY(unsigned int i, unsigned int j, unsigned int k) const {return(DerivXYZ(i,j,k,1));}
  T DerivZ(unsigned int i, unsigned int j, unsigned int k) const {return(DerivXYZ(i,j,k,2));}
  void Grad3D(unsigned int i, unsigned int j, unsigned int k, T *xg, T *yg, T *zg) const;
  void Grad(const std::vector<unsigned int>& indx, std::vector<T>& grad) const;

  // Return continous addition (since previous voxel) of integral at voxel centres
  T IntX() const;
  T IntY() const;
  T IntZ() const;

  //
  // The "useful" functionality pretty much ends here.
  // Remaining functions are mainly for debugging/diagnostics.
  //
  unsigned int NDim() const { return(_ndim); }
  unsigned int Order() const { return(_order); }
  ExtrapolationType Extrapolation(unsigned int dim) const 
  { 
    if (dim >= _ndim) throw SplinterpolatorException("Extrapolation: Invalid dimension");
    return(_et[dim]);
  } 
  const std::vector<unsigned int>& Size() const { return(_dim); }
  unsigned int Size(unsigned int dim) const { if (dim > 4) return(0); else return(_dim[dim]);}
  T Coef(unsigned int x, unsigned int y=0, unsigned int z=0) const
  {
    std::vector<unsigned int> indx(3,0);
    indx[0] = x; indx[1] = y; indx[2] = z;
    return(Coef(indx));
  }
  T Coef(std::vector<unsigned int> indx) const;
  NEWMAT::ReturnMatrix CoefAsNewmatMatrix() const;
  NEWMAT::ReturnMatrix KernelAsNewmatMatrix(double sp=0.1, unsigned int deriv=0) const;
  
  //
  // Here we declare nested helper-class SplineColumn
  //
  class SplineColumn
  {
  public:
    // Constructor
    SplineColumn(unsigned int sz, unsigned int step) : _sz(sz), _step(step) { _col = new double[_sz]; }
    // Destructor
    ~SplineColumn() { delete [] _col; }
    // Extract a column from a volume
    void Get(const T  *dp)
    {
      for (unsigned int i=0; i<_sz; i++, dp+=_step) _col[i] = static_cast<double>(*dp);
    }
    // Insert column into volume
    void Set(T *dp) const
    {
      T   test = static_cast<T>(1.5);
      if (test == 1) {  // If T is not float or double
        for (unsigned int i=0; i<_sz; i++, dp+=_step) *dp = static_cast<T>(_col[i] + 0.5);   // Round to nearest integer
      }
      else {
        for (unsigned int i=0; i<_sz; i++, dp+=_step) *dp = static_cast<T>(_col[i]);
      }
    }
    // Deconvolve column
    void Deconv(unsigned int order, ExtrapolationType et, double prec);

  private:
    unsigned int  _sz;
    unsigned int  _step;
    double        *_col;

    unsigned int get_poles(unsigned int order, double *z, unsigned int *sf) const;
    double init_bwd_sweep(double z, double lv, ExtrapolationType et, double prec) const;
    double init_fwd_sweep(double z, ExtrapolationType et, double prec) const;

    SplineColumn(const SplineColumn& sc);              // Don't allow copy-construction
    SplineColumn& operator=(const SplineColumn& sc);   // Dont allow assignment
  };
  //
  // Here ends nested helper-class SplineColumn
  //

private:
  bool                            _valid;         // Decides if neccessary information has been set or not
  bool                            _own_coef;      // Decides if we "own" (have allocated) _coef
  T                               *_coef;         // Volume of spline coefficients
  const T                         *_cptr;         // Pointer to constant data. Used instead of _coef when we don't copy the data
  unsigned int                    _order;         // Order of splines
  unsigned int                    _ndim;          // # of non-singleton dimensions
  double                          _prec;          // Precision when dealing with edges
  std::vector<unsigned int>       _dim;           // Dimensions of data
  std::vector<ExtrapolationType>  _et;            // How to do extrapolation

  //
  // Private helper-functions
  //
  void common_construction(const T *data, const std::vector<unsigned int>& dim, unsigned int order, double prec, const std::vector<ExtrapolationType>& et, bool copy);
  void assign(const Splinterpolator<T>& src);
  bool calc_coef(const T *data, bool copy);
  void deconv_along(unsigned int dim);
  T coef(int *indx) const;
  const T* coef_ptr() const {if (_own_coef) return(_coef); else return(_cptr); }
  unsigned int indx2indx(int indx, unsigned int d) const;
  unsigned int indx2linear(int k, int l, int m) const;
  unsigned int add2linear(unsigned int lin, int j) const;
  double value_at(const double *coord) const;
  double value_and_derivatives_at(const double *coord, const unsigned int *deriv, double *dval) const; 
  void derivatives_at_i(const unsigned int *indx, const unsigned int *deriv, double *dval) const;
  unsigned int get_start_indicies(const double *coord, int *sinds) const;
  unsigned int get_start_indicies_at_i(const unsigned int *indx, int *sinds) const;
  unsigned int get_wgts(const double *coord, const int *sinds, double **wgts) const;
  unsigned int get_wgts_at_i(const unsigned int *indx, const int *sinds, double **wgts) const;
  unsigned int get_dwgts(const double *coord, const int *sinds, const unsigned int *deriv, double **dwgts) const;
  unsigned int get_dwgts_at_i(const unsigned int *indx, const int *sinds, const unsigned int *deriv, double **dwgts) const;
  double get_wgt(double x) const;
  double get_wgt_at_i(int i) const;
  double get_dwgt(double x) const;
  double get_dwgt_at_i(int i) const;
  void get_dwgt1(const double * const *wgts, const double * const *dwgts, const unsigned int *dd, unsigned int nd, 
                 unsigned int k, unsigned int l, unsigned int m, double wgt1, double *dwgt1) const;

  std::pair<double,double> range() const;
  bool should_be_zero(const double *coord) const;
  unsigned int n_nonzero(const unsigned int *vec) const;
  bool odd(unsigned int i) const {return(static_cast<bool>(i%2));}
  bool even(unsigned int i) const {return(!odd(i));}

  //
  // Disallowed member functions
  //
  // Splinterpolator(const Splinterpolator& s);             // Don't allow copy-construction
  // Splinterpolator& operator=(const Splinterpolator& s);  // Don't allow assignment
};

/////////////////////////////////////////////////////////////////////
//
// Here starts public member functions for Splinterpolator
//
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
//
// Returns interpolated value at location coord.
//
/////////////////////////////////////////////////////////////////////

template<class T>
T Splinterpolator<T>::operator()(const std::vector<float>& coord) const
{
  if (!_valid) throw SplinterpolatorException("operator(): Cannot interpolate un-initialized object");
  if (coord.size() != _ndim) throw SplinterpolatorException("operator(): coord has wrong length");
  double dcoord[5] = {0.0,0.0,0.0,0.0,0.0};
  for (unsigned int i=0; i<coord.size(); i++) dcoord[i] = coord[i];
  return(static_cast<T>(value_at(dcoord)));
}

/////////////////////////////////////////////////////////////////////
//
// Returns interpolated value and a single derivative at location coord.
// The derivative should be specified as the # of the dimension
// (starting at zero) that you want it along.
//
/////////////////////////////////////////////////////////////////////

template<class T>
T Splinterpolator<T>::operator()(const std::vector<float>& coord, unsigned int dd, T *dval) const
{
  if (!_valid) throw SplinterpolatorException("operator(): Cannot interpolate un-initialized object");
  if (coord.size() != _ndim) throw SplinterpolatorException("operator(): coord has wrong length");
  if (dd > (_ndim-1)) throw SplinterpolatorException("operator(): derivative specified for invalid direction");

  double          dcoord[5] = {0.0,0.0,0.0,0.0,0.0};
  for (unsigned int i=0; i<coord.size(); i++) dcoord[i] = coord[i];
  unsigned int    deriv[5] = {0,0,0,0,0};
  deriv[dd] = 1;
  double          ddval = 0.0;
  T               rval;
  rval = static_cast<T>(value_and_derivatives_at(dcoord,deriv,&ddval));
  *dval = static_cast<T>(ddval);
  
  return(rval);
}	   

/////////////////////////////////////////////////////////////////////
//
// Returns interpolated value and a single derivative at location
// given by x, y and . The derivative should be specified as the # 
// of the dimension (starting at zero) that you want it along.
//
/////////////////////////////////////////////////////////////////////

template<class T>
T Splinterpolator<T>::operator()(double x, double y, double z, unsigned int dd, T *dval) const
{
  if (!_valid) throw SplinterpolatorException("operator(): Cannot interpolate un-initialized object");
  if (_ndim>3 || (z && _ndim<3) || (y && _ndim<2)) throw SplinterpolatorException("operator(): input has wrong dimensionality");
  if (dd > (_ndim-1)) throw SplinterpolatorException("operator(): derivative specified for invalid direction");

  double          coord[5] = {x,y,z,0.0,0.0};
  unsigned int    deriv[5] = {0,0,0,0,0};
  deriv[dd] = 1;
  double          ddval = 0.0;
  T               rval;
  rval = static_cast<T>(value_and_derivatives_at(coord,deriv,&ddval));
  *dval = static_cast<T>(ddval);
  
  return(rval);
}	   

/////////////////////////////////////////////////////////////////////
//
// Returns interpolated value and selected (by deriv) derivatives 
// at location given by coord. The interpolated value is the return
// value and the derivatives are returned in rderiv. The input
// deriv should be an _ndim long vector where a 1 indicates that
// the derivative is required in that direction and a zero that it
// is not. 
//
/////////////////////////////////////////////////////////////////////

template<class T>
T Splinterpolator<T>::ValAndDerivs(const std::vector<float>& coord, const std::vector<unsigned int>& deriv, std::vector<T>& rderiv) const
{
  if (!_valid) throw SplinterpolatorException("ValAndDerivs: Cannot interpolate un-initialized object");
  if (coord.size() != _ndim || deriv.size() != _ndim) throw SplinterpolatorException("ValAndDerivs: input has wrong dimensionality");
  double        lcoord[5] = {0.0,0.0,0.0,0.0,0.0};
  unsigned int  lderiv[5] = {0,0,0,0,0};
  unsigned int  nd = 0;
  for (unsigned int i=0; i<coord.size(); i++) { lcoord[i] = coord[i]; nd += (lderiv[i]=(deriv[i])?1:0); }
  if (rderiv.size()!=nd) SplinterpolatorException("ValAndDerivs: mismatch between deriv and rderiv");
  double        dval[5];
  T rval = static_cast<T>(value_and_derivatives_at(lcoord,lderiv,dval));
  for (unsigned int i=0; i<nd; i++) rderiv[i] = static_cast<T>(dval[i]);
  return(rval);
}

/////////////////////////////////////////////////////////////////////
//
// Returns interpolated value and derivatives in the x, y and z
// directions at a location given by x, y and z. The interpolated 
// value is the return value and the derivatives are returned in rderiv. 
//
/////////////////////////////////////////////////////////////////////

template<class T>
T Splinterpolator<T>::ValAndDerivs(double x, double y, double z, std::vector<T>& rderiv) const
{
  if (!_valid) throw SplinterpolatorException("ValAndDerivs: Cannot interpolate un-initialized object");
  if (_ndim != 3 || rderiv.size() != _ndim) throw SplinterpolatorException("ValAndDerivs: input has wrong dimensionality");
  double        coord[5] = {x,y,z,0.0,0.0};
  unsigned int  deriv[5] = {1,1,1,0,0};
  double        dval[3];
  T rval = static_cast<T>(value_and_derivatives_at(coord,deriv,dval));
  for (unsigned int i=0; i<3; i++) rderiv[i] = static_cast<T>(dval[i]);
  return(rval);
}


/////////////////////////////////////////////////////////////////////
//
// Routine that returns a 3D gradient at an integer location.
//
/////////////////////////////////////////////////////////////////////


/////////////////////////////////////////////////////////////////////
//
// Routine that returns a single derivative at an integer location.
//
/////////////////////////////////////////////////////////////////////

template<class T>
T Splinterpolator<T>::Deriv(const std::vector<unsigned int>& indx, unsigned int dd) const
{
  if (!_valid) throw SplinterpolatorException("Deriv: Cannot take derivative of un-initialized object");
  if (indx.size() != _ndim) SplinterpolatorException("Deriv: Input indx of wrong dimension");
  if (dd > (_ndim-1)) throw SplinterpolatorException("Deriv: derivative specified for invalid direction");
  double dval;
  unsigned int lindx[5] = {0,0,0,0,0};
  unsigned int deriv[5] = {0,0,0,0,0};
  for (unsigned int i=0; i<_ndim; i++) lindx[i]=indx[i];
  deriv[dd] = 1;
  derivatives_at_i(lindx,deriv,&dval);
  return(static_cast<T>(dval));
}

template<class T>
T Splinterpolator<T>::DerivXYZ(unsigned int i, unsigned int j, unsigned int k, unsigned int dd) const
{
  if (!_valid) throw SplinterpolatorException("DerivXYZ: Cannot take derivative of un-initialized object");
  if (_ndim!=3 || dd>2) throw SplinterpolatorException("DerivXYZ: Input has wrong dimensionality");
  double dval;
  unsigned int lindx[5] = {i,j,k,0,0};
  unsigned int deriv[5] = {0,0,0,0,0};
  deriv[dd] = 1;
  derivatives_at_i(lindx,deriv,&dval);
  return(static_cast<T>(dval));
}

template<class T>
void Splinterpolator<T>::Grad3D(unsigned int i, unsigned int j, unsigned int k, T *xg, T *yg, T *zg) const
{
  if (!_valid) throw SplinterpolatorException("Grad3D: Cannot take derivative of un-initialized object");
  if (_ndim != 3) SplinterpolatorException("Grad3D: Input of wrong dimension");
  unsigned int lindx[5] = {i,j,k,0,0};
  unsigned int deriv[5] = {1,1,1,0,0};
  double       dval[5] = {0.0,0.0,0.0,0.0,0.0};
  derivatives_at_i(lindx,deriv,dval);
  *xg=static_cast<T>(dval[0]); *yg=static_cast<T>(dval[1]); *zg=static_cast<T>(dval[2]);
  return; 
}

template<class T>
void Splinterpolator<T>::Grad(const std::vector<unsigned int>& indx, std::vector<T>& grad) const
{
  if (!_valid) throw SplinterpolatorException("Grad: Cannot take derivative of un-initialized object");
  if (indx.size() != _ndim || grad.size() != _ndim) SplinterpolatorException("Grad: Input indx or grad of wrong dimension");
  unsigned int lindx[5] = {0,0,0,0,0};
  unsigned int deriv[5] = {0,0,0,0,0};
  double       dval[5] = {0.0,0.0,0.0,0.0,0.0};
  for (unsigned int i=0; i<_ndim; i++) { lindx[i]=indx[i]; deriv[i]=1; }
  derivatives_at_i(lindx,deriv,dval);
  for (unsigned int i=0; i<_ndim; i++) grad[i] = static_cast<T>(dval[i]);
  return;
}

/////////////////////////////////////////////////////////////////////
//
// Returns the value of the coefficient given by indx (zero-offset)
//
/////////////////////////////////////////////////////////////////////

template<class T>
T Splinterpolator<T>::Coef(std::vector<unsigned int> indx) const
{
  if (!_valid) throw SplinterpolatorException("Coef: Cannot get coefficients for un-initialized object");
  if (!indx.size()) throw SplinterpolatorException("Coef: indx has zeros dimensions");
  if (indx.size() > 5) throw SplinterpolatorException("Coef: indx has more than 5 dimensions");
  for (unsigned int i=0; i<indx.size(); i++) if (indx[i] >= _dim[i]) throw SplinterpolatorException("Coef: indx out of range");

  unsigned int lindx=indx[indx.size()-1];
  for (int i=indx.size()-2; i>=0; i--) lindx = _dim[i]*lindx + indx[i];
  return(coef_ptr()[lindx]);
}

/////////////////////////////////////////////////////////////////////
//
// Returns the values of all coefficients as a Newmat matrix. If 
// _ndim==1 it will return a row-vector, if _ndim==2 it will return
// a matrix, if _ndim==3 it will return a tiled matrix where the n
// first rows (where n is the number of rows in one slice) pertain to 
// the first slice, the next n rows to the second slice, etc. And 
// correspondingly for 4- and 5-D.
//
/////////////////////////////////////////////////////////////////////

template<class T>
NEWMAT::ReturnMatrix Splinterpolator<T>::CoefAsNewmatMatrix() const
{
  if (!_valid) throw SplinterpolatorException("CoefAsNewmatMatrix: Cannot get coefficients for un-initialized object");
  NEWMAT::Matrix   mat(_dim[1]*_dim[2]*_dim[3]*_dim[4],_dim[0]);
  std::vector<unsigned int>  cindx(5,0);

  unsigned int r=0;
  for (cindx[4]=0; cindx[4]<_dim[4]; cindx[4]++) {
    for (cindx[3]=0; cindx[3]<_dim[3]; cindx[3]++) {
      for (cindx[2]=0; cindx[2]<_dim[2]; cindx[2]++) {
	for (cindx[1]=0; cindx[1]<_dim[1]; cindx[1]++, r++) {
	  for (cindx[0]=0; cindx[0]<_dim[0]; cindx[0]++) {
            mat.element(r,cindx[0]) = Coef(cindx);  
	  }
	}
      }
    }
  }
  mat.Release();
  return(mat);
}

/////////////////////////////////////////////////////////////////////
//
// Return the kernel matrix to verify its correctness.
//
/////////////////////////////////////////////////////////////////////

template<class T>
NEWMAT::ReturnMatrix Splinterpolator<T>::KernelAsNewmatMatrix(double sp,                // Distance (in ksp) between points 
                                                              unsigned int deriv) const // Derivative (only 0/1 implemented).
{
  if (!_valid) throw SplinterpolatorException("KernelAsNewmatMatrix: Cannot get kernel for un-initialized object");
  if (deriv > 1) throw SplinterpolatorException("KernelAsNewmatMatrix: only 1st derivatives implemented");

  std::pair<double,double>           rng = range();

  unsigned int i=0;
  for (double x=rng.first; x<=rng.second; x+=sp, i++) ; // Intentional
  NEWMAT::Matrix  kernel(i,2);
  for (double x=rng.first, i=0; x<=rng.second; x+=sp, i++) {
    kernel.element(i,0) = x;
    kernel.element(i,1) = (deriv) ? get_dwgt(x) : get_wgt(x);
  }
  kernel.Release();
  return(kernel);  
}
/////////////////////////////////////////////////////////////////////
//
// Here starts public member functions for SplineColumn
//
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
//
// This function implements the forward and backwards sweep
// as defined by equation 2.5 in Unsers paper:
//
// B-spline signal processing. II. Efficiency design and applications
//
/////////////////////////////////////////////////////////////////////

template<class T>
void Splinterpolator<T>::SplineColumn::Deconv(unsigned int order, ExtrapolationType et, double prec)
{
  double         z[3] = {0.0, 0.0, 0.0};     // Poles
  unsigned int   np = 0;                     // # of poles
  unsigned int   sf;                         // Scale-factor
  np = get_poles(order,z,&sf);

  for (unsigned int p=0; p<np; p++) {
    _col[0] = init_fwd_sweep(z[p],et,prec);
    double lv = _col[_sz-1];
    // Forward sweep
    double *ptr=&_col[1];
    for (unsigned int i=1; i<_sz; i++, ptr++) *ptr += z[p] * *(ptr-1);
    _col[_sz-1] = init_bwd_sweep(z[p],lv,et,prec);
    // Backward sweep
    ptr = &_col[_sz-2];
    for (int i=_sz-2; i>=0; i--, ptr--) *ptr = z[p]*(*(ptr+1) - *ptr);
  }
  double *ptr=_col;
  for (unsigned int i=0; i<_sz; i++, ptr++) *ptr *= sf; 
}

/////////////////////////////////////////////////////////////////////
//
// Here starts private member functions for Splinterpolator
//
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
//
// Returns the interpolated value at location given by coord. 
// coord must be a pointer to an array of indicies with _ndim
// values.
//
/////////////////////////////////////////////////////////////////////
/*
template<class T>
double Splinterpolator<T>::value_at(const double *coord) const
{
  if (should_be_zero(coord)) return(0.0);

  double       iwgt[8], jwgt[8], kwgt[8], lwgt[8], mwgt[8];
  double       *wgts[] = {iwgt, jwgt, kwgt, lwgt, mwgt};
  int          inds[5];
  unsigned int ni = 0;

  ni = get_start_indicies(coord,inds);
  get_wgts(coord,inds,wgts);

  double val=0.0;
  for (int m=0, me=(_ndim>4)?ni:1; m<me; m++) { 
    for (int l=0, le=(_ndim>3)?ni:1; l<le; l++) { 
      for (int k=0, ke=(_ndim>2)?ni:1; k<ke; k++) { 
        double wgt1 = wgts[4][m]*wgts[3][l]*wgts[2][k];
        for (int j=0, je=(_ndim>1)?ni:1; j<je; j++) { 
          double wgt2 = wgt1*wgts[1][j];
          for (int i=0; i<static_cast<int>(ni); i++) {
            int cindx[] = {inds[0]+i,inds[1]+j,inds[2]+k,inds[3]+l,inds[4]+m};
            val += coef(cindx)*wgts[0][i]*wgt2;
	  }
	}
      }
    }
  }
  return(val);
}
*/
template<class T>
double Splinterpolator<T>::value_at(const double *coord) const
{
  if (should_be_zero(coord)) return(0.0);

  double       iwgt[8], jwgt[8], kwgt[8], lwgt[8], mwgt[8];
  double       *wgts[] = {iwgt, jwgt, kwgt, lwgt, mwgt};
  int          inds[5];
  unsigned int ni = 0;
  const T      *cptr = coef_ptr();

  ni = get_start_indicies(coord,inds);
  get_wgts(coord,inds,wgts);

  double val=0.0;
  for (unsigned int m=0, me=(_ndim>4)?ni:1; m<me; m++) { 
    for (unsigned int l=0, le=(_ndim>3)?ni:1; l<le; l++) { 
      for (unsigned int k=0, ke=(_ndim>2)?ni:1; k<ke; k++) { 
        double wgt1 = wgts[4][m]*wgts[3][l]*wgts[2][k];
        unsigned int linear1 = indx2linear(inds[2]+k,inds[3]+l,inds[4]+m);
        for (unsigned int j=0, je=(_ndim>1)?ni:1; j<je; j++) { 
          double wgt2 = wgt1*wgts[1][j];
          int linear2 = add2linear(linear1,inds[1]+j);
          double *iiwgt=iwgt;
          for (unsigned int i=0; i<ni; i++, iiwgt++) {
	    val += cptr[linear2+indx2indx(inds[0]+i,0)]*(*iiwgt)*wgt2;
          }
	}
      }
    }
  }
  return(val);
}

/////////////////////////////////////////////////////////////////////
//
// Returns the interpolated value and selected derivatives at a 
// location given by coord. coord must be a pointer to an array 
// of voxel indicies with _ndim values. deriv must be a pointer
// to an _ndim long array of 0/1 specifying if the derivative is 
// requested in that direction or not.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::value_and_derivatives_at(const double       *coord,  
						    const unsigned int *deriv, 
						    double             *dval) 
const
{
  if (should_be_zero(coord)) { memset(dval,0,n_nonzero(deriv)*sizeof(double)); return(0.0); }

  double       iwgt[8], jwgt[8], kwgt[8], lwgt[8], mwgt[8];
  double       *wgts[] = {iwgt, jwgt, kwgt, lwgt, mwgt};
  double       diwgt[8], djwgt[8], dkwgt[8], dlwgt[8], dmwgt[8];
  double       *dwgts[] = {diwgt, djwgt, dkwgt, dlwgt, dmwgt};
  double       dwgt1[5];
  double       dwgt2[5];
  int          inds[5];
  unsigned int dd[5]; 
  unsigned int nd = 0;
  unsigned int ni = 0;
  const T      *cptr = coef_ptr();

  ni = get_start_indicies(coord,inds);
  get_wgts(coord,inds,wgts);
  get_dwgts(coord,inds,deriv,dwgts);
  for (unsigned int i=0; i<_ndim; i++) if (deriv[i]) { dd[nd] = i; dval[nd++] = 0.0; }

  double val=0.0;
  for (unsigned int m=0, me=(_ndim>4)?ni:1; m<me; m++) { 
    for (unsigned int l=0, le=(_ndim>3)?ni:1; l<le; l++) { 
      for (unsigned int k=0, ke=(_ndim>2)?ni:1; k<ke; k++) { 
        double wgt1 = wgts[4][m]*wgts[3][l]*wgts[2][k];
        get_dwgt1(wgts,dwgts,dd,nd,k,l,m,wgt1,dwgt1);
        unsigned int linear1 = indx2linear(inds[2]+k,inds[3]+l,inds[4]+m);
        for (unsigned int j=0, je=(_ndim>1)?ni:1; j<je; j++) { 
          double wgt2 = wgt1*wgts[1][j];
          for (unsigned int d=0; d<nd; d++) dwgt2[d] = (dd[d]==1) ? dwgt1[d]*dwgts[1][j] : dwgt1[d]*wgts[1][j];
          int linear2 = add2linear(linear1,inds[1]+j);
          double *iiwgt=iwgt;
          for (unsigned int i=0; i<ni; i++, iiwgt++) {
            double c = cptr[linear2+indx2indx(inds[0]+i,0)];
            val += c*(*iiwgt)*wgt2;
            for (unsigned int d=0; d<nd; d++) {
              double add = (dd[d]==0) ? c*diwgt[i]*dwgt2[d] : c*(*iiwgt)*dwgt2[d];
              dval[d] += add;
	    } 
	  }
	}
      }
    }
  }
  return(val);
}

template <class T>
void Splinterpolator<T>::derivatives_at_i(const unsigned int *indx,
                                          const unsigned int *deriv,
                                          double             *dval) 
const
{
  double       iwgt[8], jwgt[8], kwgt[8], lwgt[8], mwgt[8];
  double       *wgts[] = {iwgt, jwgt, kwgt, lwgt, mwgt};
  double       diwgt[8], djwgt[8], dkwgt[8], dlwgt[8], dmwgt[8];
  double       *dwgts[] = {diwgt, djwgt, dkwgt, dlwgt, dmwgt};
  double       dwgt1[5];
  double       dwgt2[5];
  int          inds[5];
  unsigned int dd[5]; 
  unsigned int nd = 0;
  unsigned int ni = 0;
  const T      *cptr = coef_ptr();

  ni = get_start_indicies_at_i(indx,inds);
  get_wgts_at_i(indx,inds,wgts);
  get_dwgts_at_i(indx,inds,deriv,dwgts);
  for (unsigned int i=0; i<_ndim; i++) if (deriv[i]) { dd[nd] = i; dval[nd++] = 0.0; }

  // double val=0.0;
  for (unsigned int m=0, me=(_ndim>4)?ni:1; m<me; m++) { 
    for (unsigned int l=0, le=(_ndim>3)?ni:1; l<le; l++) { 
      for (unsigned int k=0, ke=(_ndim>2)?ni:1; k<ke; k++) { 
        double wgt1 = wgts[4][m]*wgts[3][l]*wgts[2][k];
        get_dwgt1(wgts,dwgts,dd,nd,k,l,m,wgt1,dwgt1);
        unsigned int linear1 = indx2linear(inds[2]+k,inds[3]+l,inds[4]+m);
        for (unsigned int j=0, je=(_ndim>1)?ni:1; j<je; j++) { 
          // double wgt2 = wgt1*wgts[1][j];
          for (unsigned int d=0; d<nd; d++) dwgt2[d] = (dd[d]==1) ? dwgt1[d]*dwgts[1][j] : dwgt1[d]*wgts[1][j];
          int linear2 = add2linear(linear1,inds[1]+j);
          double *iiwgt=iwgt;
          for (unsigned int i=0; i<ni; i++, iiwgt++) {
            double c = cptr[linear2+indx2indx(inds[0]+i,0)];
            // val += c*(*iiwgt)*wgt2;
            for (unsigned int d=0; d<nd; d++) {
              double add = (dd[d]==0) ? c*diwgt[i]*dwgt2[d] : c*(*iiwgt)*dwgt2[d];
              dval[d] += add;
	    } 
	  }
	}
      }
    }
  }
  // return(val);
  return;
}
    
/////////////////////////////////////////////////////////////////////
//
// Returns (in sinds) the indicies of the first coefficient in all
// _ndim directions with a non-zero weight for the location given
// by coord. The caller is responsible for coord and sinds being
// valid pointers to arrays of 5 values.
// The return-value gives the total # of non-zero weights.
//
/////////////////////////////////////////////////////////////////////

template<class T>
unsigned int Splinterpolator<T>::get_start_indicies(const double *coord, int *sinds) const
{
  unsigned int ni = _order+1;
  
  if (odd(ni)) {
    for (unsigned int i=0; i<_ndim; i++) {
      sinds[i] = static_cast<int>(coord[i]+0.5) - ni/2;
    }
  }
  else {
    for (unsigned int i=0; i<_ndim; i++) {
      int ix = static_cast<int>(coord[i]+0.5);
      if (ix < coord[i]) sinds[i] = ix - (ni-1)/2;
      else sinds[i] = ix -ni/2;
    }
  }
  for (unsigned int i=_ndim; i<5; i++) sinds[i] = 0;

  return(ni);
}

// Does the same thing, but for integer (spot on voxel centre) index

template<class T>
unsigned int Splinterpolator<T>::get_start_indicies_at_i(const unsigned int *indx, int *sinds) const
{
  unsigned int ni = (odd(_order)) ? _order : _order+1;
  for (unsigned int i=0; i<_ndim; i++) {
    sinds[i] = indx[i] - (_order/2);
  }
  for (unsigned int i=_ndim; i<5; i++) sinds[i] = 0;
  return(ni);
}

/////////////////////////////////////////////////////////////////////
//
// Returns (in wgts) the weights for the coefficients given by sinds 
// for the location given by coord. 
//
/////////////////////////////////////////////////////////////////////

template<class T>
unsigned int Splinterpolator<T>::get_wgts(const double *coord, const int *sinds, double **wgts) const
{
  unsigned int ni = _order+1;

  for (unsigned int dim=0; dim<_ndim; dim++) {
    for (unsigned int i=0; i<ni; i++) {
      wgts[dim][i] = get_wgt(coord[dim]-(sinds[dim]+i));
    }
  }
  for (unsigned int dim=_ndim; dim<5; dim++) wgts[dim][0] = 1.0;

  return(ni);
}

// Same for integer (spot on voxel centre) index
template<class T>
unsigned int Splinterpolator<T>::get_wgts_at_i(const unsigned int *indx, const int *sinds, double **wgts) const
{
  unsigned int ni = (odd(_order)) ? _order : _order+1;
  
  for (unsigned int dim=0; dim<_ndim; dim++) {
    for (unsigned int i=0; i<ni; i++) {
      wgts[dim][i] = get_wgt_at_i(indx[dim]-(sinds[dim]+i));
    }
  }
  for (unsigned int dim=_ndim; dim<5; dim++) wgts[dim][0] = 1.0;

  return(ni);
}

template<class T>
unsigned int Splinterpolator<T>::get_dwgts(const double *coord, const int *sinds, const unsigned int *deriv, double **dwgts) const
{
  unsigned int ni = _order+1;

  for (unsigned int dim=0; dim<_ndim; dim++) {
    if (deriv[dim]) {
      switch (_order) {
      case 0:
	throw SplinterpolatorException("get_dwgts: invalid order spline");
        break;
      case 1:
        dwgts[dim][0] = -1; dwgts[dim][1] = 1;  // Not correct on original gridpoints
	break;
      case 2: case 3: case 4: case 5: case 6: case 7:
	for (unsigned int i=0; i<ni; i++) {
	  dwgts[dim][i] = get_dwgt(coord[dim]-(sinds[dim]+i));
        }
	break;
      default:
	throw SplinterpolatorException("get_dwgts: invalid order spline");
      }
    }
  }

  return(ni);
}

// Same for integer (spot on voxel centre) index
template<class T>
unsigned int Splinterpolator<T>::get_dwgts_at_i(const unsigned int *indx, const int *sinds, const unsigned int *deriv, double **dwgts) const
{
  unsigned int ni = (odd(_order)) ? _order : _order+1;

  for (unsigned int dim=0; dim<_ndim; dim++) {
    if (deriv[dim]) {
      switch (_order) {
      case 0: case 1:
	throw SplinterpolatorException("get_dwgts_at_i: invalid order spline");
        break;
      case 2: case 3: case 4: case 5: case 6: case 7:
	for (unsigned int i=0; i<ni; i++) {
	  dwgts[dim][i] = get_dwgt_at_i(indx[dim]-(sinds[dim]+i));
        }
	break;
      default:
	throw SplinterpolatorException("get_dwgts_at_i: invalid order spline");
      }
    }
  }

  return(ni);
}

/////////////////////////////////////////////////////////////////////
//
// Returns the weight for a spline at integer index i, where i is 
// relative to the centre index of the spline.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::get_wgt_at_i(int i) const
{
  double val = 0.0;
  int ai = std::abs(i);
  switch (_order) {
  case 0: case 1:
    val = (ai) ? 1.0 : 0.0;
    break;
  case 2:
    if (!ai) val = 0.75;
    else if (ai==1) val = 0.125;
    break;
  case 3:
    if (!ai) val = 0.666666666666667;
    else if (ai==1) val = 0.166666666666667;
    break;
  case 4:
    if (!ai) val = 0.598958333333333;
    else if (ai==1) val = 0.197916666666667;
    else if (ai==2) val = 0.002604166666667;
    break;
  case 5:
    if (!ai) val = 0.55;
    else if (ai==1) val = 0.216666666666667;
    else if (ai==2) val = 0.008333333333333;
    break;
  case 6:
    if (!ai) val = 0.511024305555556;
    else if (ai==1) val = 0.228797743055556;
    else if (ai==2) val = 0.015668402777779;
    else if (ai==3) val = 8.680555555555556e-05;
    break;
  case 7:
    if (!ai) val = 0.479365079365079;
    else if (ai==1) val = 0.236309523809524;
    else if (ai==2) val = 0.023809523809524;
    else if (ai==3) val = 1.984126984126984e-04;
    break;
  default:
    throw SplinterpolatorException("get_wgt_at_i: invalid order spline");
    break;
  }
  return(val);     
}

/////////////////////////////////////////////////////////////////////
//
// Returns the weight for the first derivative of a spline at integer 
// index i, where i is relative to the centre index of the spline.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::get_dwgt_at_i(int i) const
{
  double val = 0.0;
  int ai = std::abs(i);
  int sign = (ai) ? i/ai : 1;

  switch (_order) {
  case 0: case 1:
    throw SplinterpolatorException("get_dwgt: invalid order spline");
    break;
  case 2:
    if (!ai) val = 0.0;
    else if (ai==1) val = sign * (-0.5);
    break;
  case 3:
    if (!ai) val = 0.0;
    else if (ai==1) val = sign * (-0.5);
    break;
  case 4:
    if (!ai) val = 0.0;
    else if (ai==1) val = sign * (-0.458333333333333);
    else if (ai==2) val = sign * (-0.020833333333333);
    break;
  case 5:
    if (!ai) val = 0.0;
    else if (ai==1) val = sign * (-0.416666666666667);
    else if (ai==2) val = sign * (-0.041666666666667);
    break;
  case 6:
    if (!ai) val = 0.0;
    else if (ai==1) val = sign * (-0.376302083333333);
    else if (ai==2) val = sign * (-0.061458333333334);
    else if (ai==3) val = sign * (-2.604166666666667e-04);
    break;
  case 7:
    if (!ai) val = 0.0;
    else if (ai==1) val = sign * (-0.340277777777778);
    else if (ai==2) val = sign * (-0.077777777777778);
    else if (ai==3) val = sign * (-0.001388888888889);
    break;
  default:
    throw SplinterpolatorException("get_dwgt_at_i: invalid order spline");
    break;
  }
  return(val);     
}

/////////////////////////////////////////////////////////////////////
//
// Returns the weight for a spline at coordinate x, where x is relative
// to the centre of the spline.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::get_wgt(double x) const
{
  double val = 0.0;
  double ax = abs(x);  // Kernels all symmetric

  switch (_order) {
  case 0:
    if (ax < 0.5) val = 1.0;
    break;
  case 1:
    if (ax < 1) val = 1-ax;;
    break;
  case 2:
    if (ax < 0.5) val = 0.75-ax*ax;
    else if (ax < 1.5) val = 0.5*(1.5-ax)*(1.5-ax);
    break;
  case 3:
    if (ax < 1) val = 2.0/3.0 + 0.5*ax*ax*(ax-2);
    else if (ax < 2) { ax = 2-ax; val = (1.0/6.0)*(ax*ax*ax); }
    break;
  case 4:
    if (ax < 0.5) { ax *= ax; val = (115.0/192.0) + ax*((2.0*ax-5.0)/8.0); }
    else if (ax < 1.5) val = (55.0/96.0) + ax*(ax*(ax*((5.0-ax)/6.0) - 1.25) + 5.0/24.0);
    else if (ax < 2.5) { ax -= 2.5; ax *= ax; val = (1.0/24.0)*ax*ax; }
    break;
  case 5:
    if (ax < 1) { double xx = ax*ax; val = 0.55 + xx*(xx*((3.0-ax)/12.0) - 0.5); }
    else if (ax < 2) val = 0.425 + ax*(ax*(ax*(ax*((ax-9.0)/24.0) + 1.25) - 1.75) + 0.625);
    else if (ax < 3) { ax = 3-ax; double xx = ax*ax; val = (1.0/120.0)*ax*xx*xx; }
    break;
  case 6:
    if (ax < 0.5) { ax *= ax; val = (5887.0/11520.0) + ax*(ax*((21.0-4.0*ax)/144.0) -77.0/192.0); }
    else if (ax < 1.5) val = 7861.0/15360.0 + ax*(ax*(ax*(ax*(ax*((ax - 7.0)/48.0) + 0.328125) - 35.0/288.0) - 91.0/256.0) -7.0/768.0);
    else if (ax < 2.5) val = 1379.0/7680.0 + ax*(ax*(ax*(ax*(ax*((14.0-ax)/120.0) - 0.65625) + 133.0/72.0) - 2.5703125) + 1267.0/960.0);
    else if (ax < 3.5) { ax -= 3.5; ax *= ax*ax; val = (1.0/720.0) * ax*ax; }
    break;
  case 7:
    if (ax < 1) { double xx = ax*ax; val = 151.0/315.0 + xx*(xx*(xx*((ax-4.0)/144.0) + 1.0/9.0) - 1.0/3.0); }
    else if (ax < 2) val = 103.0/210.0 + ax*(ax*(ax*(ax*(ax*(ax*((12.0-ax)/240.0) -7.0/30.0) + 0.5) - 7.0/18.0) - 0.1) -7.0/90.0);
    else if (ax < 3) val = ax*(ax*(ax*(ax*(ax*(ax*((ax-20.0)/720.0) + 7.0/30.0) - 19.0/18.0) + 49.0/18.0) - 23.0/6.0) + 217.0/90.0) - 139.0/630.0;
    else if (ax < 4) { ax = 4-ax; double xxx=ax*ax*ax; val = (1.0/5040.0)*ax*xxx*xxx; }
    break;
  default:
    throw SplinterpolatorException("get_wgt: invalid order spline");
    break;
  }

  return(val);
}

/////////////////////////////////////////////////////////////////////
//
// Returns the weight for the first derivative of a spline at 
// coordinate x, where x is relative to the centre of the spline.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::get_dwgt(double x) const
{
  double val = 0.0;
  double ax = abs(x);                               // Kernels all anti-symmetric
  int    sign = (ax) ? static_cast<int>(x/ax) : 1;  // Arbitrary choice for when x=0

  switch (_order) {
  case 0: case 1:
    throw SplinterpolatorException("get_dwgt: invalid order spline");
    break;
  case 2:
    if (ax < 0.5) val = sign * -2.0*ax;
    else if (ax < 1.5) val = sign * (-1.5 + ax);
    break;
  case 3:
    if (ax < 1) val = sign * (1.5*ax*ax - 2.0*ax);
    else if (ax < 2) { ax = 2-ax; val = sign * -0.5*ax*ax; }
    break;
  case 4:
    if (ax < 0.5) val = sign * (ax*ax*ax - 1.25*ax);
    else if (ax < 1.5) val = sign * (5.0/24.0 - ax*(2.5 - ax*(2.5 - (2.0/3.0)*ax)));
    else if (ax < 2.5) { ax -= 2.5; val = sign * (1.0/6.0)*ax*ax*ax; }
    break;
  case 5:
    if (ax < 1) val = sign * ax*(ax*(ax*(1-(5.0/12.0)*ax)) - 1);
    else if (ax < 2) val = sign * (0.625 - ax*(3.5 - ax*(3.75 - ax*(1.5 - (5.0/24.0)*ax))));
    else if (ax < 3) { ax -= 3; ax = ax*ax; val = sign * (-1.0/24.0)*ax*ax; }
    break;
  case 6:
    if (ax < 0.5) { double xx = ax*ax; val = sign * ax*(xx*((7.0/12) - (1.0/6.0)*xx) - (77.0/96.0)); }
    else if (ax < 1.5) {double xx = ax*ax; val = sign * (ax*(xx*(0.1250*xx + 1.3125) - 0.7109375) - xx*((35.0/48.0)*xx + (35.0/96.0)) - (7.0/768.0)); }
    else if (ax < 2.5) { double xx = ax*ax; val = sign * ((1267.0/960.0) - ax*(xx*(0.05*xx + (21.0/8.0)) + (329.0/64.0)) + xx*((7.0/12.0)*xx + (133.0/24.0))); }
    else if (ax < 3.5) { ax -= 3.5; double xx = ax*ax; val = sign * (1.0/120.0)*xx*xx*ax; }
    break;
  case 7:
    if (ax < 1) { double xx = ax*ax; val = sign * ax*(xx*(xx*((7.0/144.0)*ax - (1.0/6.0)) + 4.0/9.0) - 2.0/3.0); }
    else if (ax < 2) { double xx = ax*ax; val = sign * (ax*(xx*(xx*0.3 + 2.0) - 0.2) - xx*(xx*(xx*(7.0/240.0) + (7.0/6.0)) + (7.0/6.0)) - (7.0/90.0)); }
    else if (ax < 3) { double xx = ax*ax; val = sign * (1.0/720.0)*(xx - 4.0*ax + 2.0)*(7.0*xx*xx - 92.0*xx*ax + 458.0*xx - 1024.0*ax + 868.0); }
    else if (ax < 4) { ax = 4-ax; ax = ax*ax*ax; val = sign * (-1.0/720.0)*ax*ax; }
    break;
  default:
    throw SplinterpolatorException("get_dwgt: invalid order spline");
    break;
  }

  return(val);
}

template<class T>
inline void Splinterpolator<T>::get_dwgt1(const double * const *wgts, const double * const *dwgts, 
                                          const unsigned int *dd, unsigned int nd, unsigned int k, 
                                          unsigned int l, unsigned int m, double wgt1, double *dwgt1) const
{
  for (unsigned int i=0; i<nd; i++) {
    switch (dd[i]) {
    case 2:
      dwgt1[i] = wgts[4][m] * wgts[3][l] * dwgts[2][k];
      break;
    case 3:
      dwgt1[i] = wgts[4][m] * dwgts[3][l] * wgts[2][k];
      break;
    case 4:
      dwgt1[i] = dwgts[4][m] * wgts[3][l] * wgts[2][k];
      break;
    default:
      dwgt1[i] = wgt1;
      break;
    }
  }
}

template<class T>
inline std::pair<double,double> Splinterpolator<T>::range() const
{
  std::pair<double,double>  rng(0.0,0.0);
  rng.second = static_cast<double>(_order+1.0)/2.0;
  rng.first = - rng.second;
  return(rng);
}
/////////////////////////////////////////////////////////////////////
//
// Returns the value of the coefficient indexed by indx. Unlike the
// public Coef() this routine allows indexing outside the valid
// volume, returning values that are dependent on the extrapolation
// model when these are encountered.
//
// N.B. May change value of input index N.B.
//
/////////////////////////////////////////////////////////////////////

template<class T>
inline unsigned int Splinterpolator<T>::indx2indx(int indx, unsigned int d) const
{
  if (d > (_ndim-1)) return(0);

  if (indx < 0) {
    switch (_et[d]) {
    case Constant:
      return(0);
      break;
    case Zeros: case Mirror:
      return((indx%int(_dim[d])) ? _dim[d]-1 : -1-indx%int(_dim[d]));
      break;
    case Periodic:
      return((indx%int(_dim[d])) ? _dim[d]+indx%int(_dim[d]) : 0);
      break;
    default:
      break;
    }
  }
  else if (indx >= static_cast<int>(_dim[d])) {
    switch (_et[d]) {
    case Constant:
      return(_dim[d]-1);
      break;
    case Zeros: case Mirror:
      return(2*_dim[d] - (_dim[d]+indx%int(_dim[d])) - 1);
      break;
    case Periodic:
      return(indx%int(_dim[d]));
      break;
    default:
      break;
    }
  }
  return(indx);
}

template<class T>
unsigned int Splinterpolator<T>::indx2linear(int k, int l, int m) const
{
  if (_ndim < 3) return(0);

  int lindx = 0;
  if (_ndim>4) lindx = indx2indx(m,4);
  if (_ndim>3) lindx = _dim[3]*lindx + indx2indx(l,3);
  lindx = _dim[0]*_dim[1]*(_dim[2]*lindx + indx2indx(k,2));

  return(lindx);
}

template<class T>
inline unsigned int Splinterpolator<T>::add2linear(unsigned int lin, int j) const
{
  if (_ndim < 2) return(lin);
  else return(lin + _dim[0]*indx2indx(j,1));
}

template<class T>
T Splinterpolator<T>::coef(int *indx) const
{
  // First fix any outside-volume indicies
  for (unsigned int i=0; i<_ndim; i++) {
    if (indx[i] < 0) {
      switch (_et[i]) {
      case Zeros:
	return(static_cast<T>(0));
	break;
      case Constant:
	indx[i] = 0;
	break;
      case Mirror:
	indx[i] = 1-indx[i];
	break;
      case Periodic:
	indx[i] = _dim[i]+indx[i];
	break;
      default:
	break;
      }
    }
    else if (indx[i] >= static_cast<int>(_dim[i])) {
      switch (_et[i]) {
      case Zeros:
	return(static_cast<T>(0));
	break;
      case Constant:
	indx[i] = _dim[i]-1;
	break;
      case Mirror:
	indx[i] = 2*_dim[i]-indx[i]-1;
	break;
      case Periodic:
	indx[i] = indx[i]-_dim[i];
	break;
      default:
	break;
      }
    }
  }
  // Now make linear index
  unsigned int lindx=indx[_ndim-1];
  for (int i=_ndim-2; i>=0; i--) lindx = _dim[i]*lindx + indx[i];
  return(coef_ptr()[lindx]);
}

template<class T>
bool Splinterpolator<T>::should_be_zero(const double *coord) const
{
  for (unsigned int i=0; i<_ndim; i++) {
    if (_et[i] == Zeros && (coord[i] < 0 || coord[i] > (_dim[i]-1))) return(true);
  }
  return(false);
}

template<class T>
unsigned int Splinterpolator<T>::n_nonzero(const unsigned int *vec) const
{
  unsigned int n=0;
  for (unsigned int i=0; i<_ndim; i++) if (vec[i]) n++;
  return(n);
}

/////////////////////////////////////////////////////////////////////
//
// Takes care of the "common" tasks when constructing a 
// Splinterpolator object. Called by constructors and by .Set()
//
/////////////////////////////////////////////////////////////////////

template<class T>
void Splinterpolator<T>::common_construction(const T *data, const std::vector<unsigned int>& dim, unsigned int order, double prec, const std::vector<ExtrapolationType>& et, bool copy)
{
  if (!dim.size()) throw SplinterpolatorException("common_construction: data has zeros dimensions");
  if (!dim.size() > 5) throw SplinterpolatorException("common_construction: data cannot have more than 5 dimensions");
  if (dim.size() != et.size()) throw SplinterpolatorException("common_construction: dim and et must have the same size");
  for (unsigned int i=0; i<dim.size(); i++) if (!dim[i]) throw SplinterpolatorException("common_construction: data cannot have zeros size in any direction");
  if (order > 7) throw SplinterpolatorException("common_construction: spline order must be lesst than 7");
  if (!data) throw SplinterpolatorException("common_construction: zero data pointer");
  
  _order = order;
  _prec = prec;
  _et = et;
  _dim.resize(5);
  _ndim = dim.size();
  for (unsigned int i=0; i<5; i++) _dim[i]  = (i < dim.size()) ? dim[i] : 1;

  _own_coef = calc_coef(data,copy);

  _valid = true;
}

/////////////////////////////////////////////////////////////////////
//
// Takes care of the "common" tasks when copy-constructing
// and when assigning.
//
/////////////////////////////////////////////////////////////////////

template<class T>
void Splinterpolator<T>::assign(const Splinterpolator<T>& src)
{
  _valid = src._valid;
  _own_coef = src._own_coef;
  _cptr = src._cptr;
  _order = src._order;
  _ndim = src._ndim;
  _prec = src._prec;
  _dim = src._dim;
  _et = src._et;
  
  if (_own_coef) { // If we need to do a deep copy
    unsigned int ts = 1;
    for (unsigned int i=0; i<_ndim; i++) ts *= _dim[i];
    _coef = new T[ts];
    memcpy(_coef,src._coef,ts*sizeof(T));
  }
}

/////////////////////////////////////////////////////////////////////
//
// Performs deconvolution, converting signal to spline coefficients.
//
/////////////////////////////////////////////////////////////////////

template<class T>
bool Splinterpolator<T>::calc_coef(const T *data, bool copy)
{
  if (_order < 2 && !copy) { _cptr = data; return(false); }

  // Allocate memory and put the original data into _coef
  //
  unsigned int ts=1;
  for (unsigned int i=0; i<_dim.size(); i++) ts *= _dim[i];
  _coef = new T[ts];
  memcpy(_coef,data,ts*sizeof(T));

  if (_order < 2) return(true);  // If nearest neighbour or linear, that's all we need 

  // Loop over all non-singleton dimensions and deconvolve along them
  //
  std::vector<unsigned int>  tdim(_dim.size()-1,0); 
  for (unsigned int cdir=0; cdir<_dim.size(); cdir++) {
    if (_dim[cdir] > 1) deconv_along(cdir);
  }

  return(true);
}

/////////////////////////////////////////////////////////////////////
//
// Performs deconvolution along one of the dimensions, visiting
// all points along the other dimensions.
//
/////////////////////////////////////////////////////////////////////

template<class T>
void Splinterpolator<T>::deconv_along(unsigned int dim)
{
  // Set up to reflect "missing" dimension
  //
  std::vector<unsigned int>   rdim(4,1);     // Sizes along remaining dimensions
  std::vector<unsigned int>   rstep(4,1);    // Step-sizes (in "volume") of remaining dimensions
  unsigned int                mdim = 1;      // Size along "missing" dimension
  unsigned int                mstep = 1;     // Step-size along "missing" dimension
  for (unsigned int i=0, j=0, ss=1; i<5; i++) {
    if (i == dim) {  // If it is our "missing" dimension
      mdim = _dim[i];
      mstep = ss;
    }
    else {
      rdim[j] = _dim[i];
      rstep[j++] = ss;
    }
    ss *= _dim[i];
  }

  SplineColumn  col(mdim,mstep);          // Column helps us do the job
  
  for (unsigned int l=0; l<rdim[3]; l++) {
    for (unsigned int k=0; k<rdim[2]; k++) {
      for (unsigned int j=0; j<rdim[1]; j++) {
        T *dp = _coef + l*rstep[3] + k*rstep[2] + j*rstep[1];
	for (unsigned int i=0; i<rdim[0]; i++, dp+=rstep[0]) {
	  col.Get(dp);                          // Extract a column from the volume
          col.Deconv(_order,_et[dim],_prec);    // Deconvolve it
          col.Set(dp);                          // Put back the deconvolved column
	}
      }
    }
  }
  return;
}

/////////////////////////////////////////////////////////////////////
//
// Here starts private member functions for SplineColumn
//
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
//
// This function returns the poles and scale-factors for splines
// of order 2--7. The values correspond to those found in
// table 1 in Unsers 1993 paper: 
// B-spline signal processing. II. Efficiency design and applications
// 
// The actual values have been taken from
// http://bigwww.epfl.ch/thevenaz/interpolation/coeff.c
//
/////////////////////////////////////////////////////////////////////

template<class T>
unsigned int Splinterpolator<T>::SplineColumn::get_poles(unsigned int order, double *z, unsigned int *sf) const
{
  unsigned int   np = 0;                     // # of poles

  switch (order) {
  case 2:
    np = 1;
    z[0] = 2.0*sqrt(2.0) - 3.0;
    *sf = 8;
    break;
  case 3:
    np = 1;
    z[0] = sqrt(3.0) - 2.0;
    *sf = 6;
    break;
  case 4:
    np = 2;
    z[0] = sqrt(664.0 - sqrt(438976.0)) + sqrt(304.0) - 19.0;
    z[1] = sqrt(664.0 + sqrt(438976.0)) - sqrt(304.0) - 19.0;
    *sf = 384;
    break;
  case 5:
    np = 2;
    z[0] = sqrt(135.0 / 2.0 - sqrt(17745.0 / 4.0)) + sqrt(105.0 / 4.0) - 13.0 / 2.0;
    z[1] = sqrt(135.0 / 2.0 + sqrt(17745.0 / 4.0)) - sqrt(105.0 / 4.0) - 13.0 / 2.0;
    *sf = 120;
    break;
  case 6:
    np = 3;
    z[0] = -0.48829458930304475513011803888378906211227916123938;
    z[1] = -0.081679271076237512597937765737059080653379610398148;
    z[2] = -0.0014141518083258177510872439765585925278641690553467;
    *sf = 46080;
    break;
  case 7:
    np = 3;
    z[0] = -0.53528043079643816554240378168164607183392315234269;
    z[1] = -0.12255461519232669051527226435935734360548654942730;
    z[2] = -0.0091486948096082769285930216516478534156925639545994;
    *sf = 5040;
    break;
  default:
    throw SplinterpolatorException("SplineColumn::get_poles: invalid order of spline");
    break;
  }
  return(np);
}

/////////////////////////////////////////////////////////////////////
//
// Initialises the first value for the forward sweep. The initialisation
// will always be an approximation (this is where the "infinite" in IIR
// breaks down) so the value will be calculated to a predefined precision.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::SplineColumn::init_fwd_sweep(double z, ExtrapolationType et, double prec) const
{
  //
  // Move logs away from here after debugging
  //
  unsigned int n = static_cast<unsigned int>((log(prec)/log(abs(z))) + 1.5);
  n = (n > _sz) ? _sz : n;

  double iv = _col[0];
  if (et == Periodic) {
    double *ptr=&_col[_sz-1];
    double z2i=z;
    for (unsigned int i=1; i<n; i++, ptr--, z2i*=z) iv += z2i * *ptr;
  }
  else {
    double *ptr=&_col[1];
    double z2i=z;
    for (unsigned int i=1; i<n; i++, ptr++, z2i*=z) iv += z2i * *ptr;
  }
  return(iv); 
}

/////////////////////////////////////////////////////////////////////
//
// Initialises the first value for the backward sweep. The initialisation
// will always be an approximation (this is where the "infinite" in IIR
// breaks down) so the value will be calculated to a predefined precision.
//
/////////////////////////////////////////////////////////////////////

template<class T>
double Splinterpolator<T>::SplineColumn::init_bwd_sweep(double z, double lv, ExtrapolationType et, double prec) const
{
  double iv = 0.0;
  if (et == Periodic) {
    unsigned int n = static_cast<unsigned int>((log(prec)/log(abs(z))) + 1.5);
    n = (n > _sz) ? _sz : n;
    iv = z * _col[_sz-1];
    double z2i = z*z;
    double *ptr=_col;
    for (unsigned int i=1; i<n; i++, ptr++, z2i*=z) {
      iv += z2i * *ptr;
    }
    iv /= (z2i-1.0);
  }
  else {
    iv = -z/(1.0-z*z) * (2.0*_col[_sz-1] - lv);
  }
  return(iv);
}

} // End namespace SPLINTERPOLATOR

#endif // End #ifndef splinterpolator.h