splinterpolator.h

//
//  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 <thread>
#include <iomanip>
#include "armawrap/newmat.h"
#include "utils/threading.h"
#include "miscmaths/miscmaths.h"

namespace SPLINTERPOLATOR {

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

class SplinterpolatorException: public std::exception
{
public:
  SplinterpolatorException(const std::string& msg) noexcept : m_msg(std::string("Splinterpolator::") + msg) {}
  ~SplinterpolatorException() noexcept {}
  virtual const char *what() const noexcept { return(m_msg.c_str()); }
private:
  std::string m_msg;
};

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

template<class T>
class Splinterpolator
{
public:
  // Constructors
  Splinterpolator() : _valid(false), _own_coef(false), _coef(0), _cptr(0), _ndim(0), _nthr(1) {}
  Splinterpolator(const T *data, const std::vector<unsigned int>& dim, const std::vector<ExtrapolationType>& et, unsigned int order=3, bool copy_low_order=true, Utilities::NoOfThreads nthr=Utilities::NoOfThreads(1), double prec=1e-8) : _valid(false), _own_coef(false), _coef(0), _cptr(0), _ndim(0), _nthr(nthr._n)
  {
    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, Utilities::NoOfThreads nthr=Utilities::NoOfThreads(1), double prec=1e-8) : _valid(false), _own_coef(false), _coef(0), _cptr(0), _ndim(0), _nthr(nthr._n)
  {
    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); }
  bool Valid() const { return(_valid); }
  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
  unsigned int                    _nthr;          // Number of threads used for the deconvolution
  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);
  void deconv_along_mt_helper(unsigned int dim, unsigned int mdim, unsigned int mstep, unsigned int offset, unsigned int step, 
			      const std::vector<unsigned int>& rdim, const std::vector<unsigned int>& rstep);						
  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;

  for (unsigned int i=0; i<_ndim; i++) {
    sinds[i] = std::ceil(coord[i]) - 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]+int(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]+int(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");
      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]+int(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");
      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]+int(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");
  }
  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");
  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");
  }
  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 = std::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;