#ifndef _ITKINTERPOLATEIMAGEFUNCTIONWITHLUT_
#define _ITKINTERPOLATEIMAGEFUNCTIONWITHLUT_

/* =========================================================================
                                                                                
  @file   itkBSplineInterpolateImageFunctionWithLUT.txx
  @author David Sarrut <david.sarrut@creatis.insa-lyon.fr>

  Copyright (c) 
  * CREATIS (Centre de Recherche et d'Applications en Traitement de l'Image). 
    All rights reserved. See Doc/License.txt or
    http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
  * L�on B�rard cancer center, 28 rue La�nnec, 69373 Lyon cedex 08, France
  * http://www.creatis.insa-lyon.fr/rio
                                                                                
  This software is distributed WITHOUT ANY WARRANTY; without even the
  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notices for more information.
                                                                             
========================================================================= */
#include "itkCastImageFilter.h"

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
BSplineInterpolateImageFunctionWithLUT():Superclass() {  
  // Set default values
  for(int i=0; i<TImageType::ImageDimension; i++) mOutputSpacing[i] = -1;
  SetLUTSamplingFactor(10);
  SetSplineOrder(3);
  mWeightsAreUpToDate = false;
  // Following need to be pointer beacause values are updated into 
  // "const" function
  mIntrinsecError  = new double;
  mNumberOfError = new long;
  mIntrinsecErrorMax = new double;
  mInputIsCoef = false;
}
//====================================================================

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
void BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
SetLUTSamplingFactor(int s) {  
  for(int i=0; i<TImageType::ImageDimension; i++) mSamplingFactors[i] = s;
  mWeightsAreUpToDate = false;
}

//====================================================================

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
void BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
SetOutputSpacing(const SpacingType & s) {
  for(int i=0; i<TImageType::ImageDimension; i++) 
    mOutputSpacing[i] = s[i];
  mWeightsAreUpToDate = false;
}
//====================================================================

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
void BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
SetSplineOrder(unsigned int SplineOrder) {
  Superclass::SetSplineOrder(SplineOrder);
  // Compute support and half size
  static const int d = TImageType::ImageDimension;
  for(int l=0; l<d; l++) {
    mSplineOrders[l]= SplineOrder;
    mSupport[l] = SplineOrder+1;
    if (mSupport[l] % 2 == 0) { // support is even
      mHalfSupport[l] = mSupport[l]/2-1;
    }
    else mHalfSupport[l] = mSupport[l]/2; // support is odd (like cubic spline)
  }
  mSupportSize = 1;
  for(int l=0; l<d; l++) {
    mSupportSize *= mSupport[l];
  }
  mWeightsAreUpToDate = false;
}
//====================================================================

//JV
//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
void BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
SetSplineOrders(SizeType SplineOrders) {
  mSplineOrders=SplineOrders;
  DD(mSplineOrders); 
// Compute support and half size
  static const int d = TImageType::ImageDimension;
  for(int l=0; l<d; l++) {
    mSupport[l] = mSplineOrders[l]+1;
    if (mSupport[l] % 2 == 0) { // support is even
      mHalfSupport[l] = mSupport[l]/2-1;
    }
    else mHalfSupport[l] = mSupport[l]/2; // support is odd (like cubic spline)
  }
  mSupportSize = 1;
  for(int l=0; l<d; l++) {
    mSupportSize *= mSupport[l];
  }
  mWeightsAreUpToDate = false;
}
//====================================================================

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
void BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
SetInputImage(const TImageType * inputData) { 

  // Call superclass SetInputImage and return if input is null
  // clitk::Timer t;
  //   t.Start();
  //ds 
  // this->InterpolateImageFunction<TImageType,TCoordRep>::SetInputImage(inputData);

  //==============================
  if (!mInputIsCoef) {
    Superclass::SetInputImage(inputData);
    //  t.Stop();
  }
  //==============================
  else {
    // If inputData is not float/double, we do not want to set
    // mInputIsCoef to true, but the compiler goes here ...

    if (typeid(typename TImageType::PixelType) != typeid(TCoefficientType)) {
      std::cerr << "Input should be float or double to be set as 'coefficients' with SetInputImageIsCoefficient(true) and should be the same type than TCoefficientType" << std::endl;
      exit(0);
    }
    else {
      itkStaticConstMacro(ImageDimension, unsigned int, Superclass::ImageDimension);
      typedef Image<TCoefficientType, ImageDimension> CoefficientImageType;
      typedef itk::CastImageFilter< TImageType, CoefficientImageType> CastFilterType;
      typename CastFilterType::Pointer castFilter=CastFilterType::New();
      castFilter->SetInput(inputData);
      castFilter->Update();
      this->m_Coefficients=castFilter->GetOutput();
      if ( this->m_Coefficients.IsNotNull())
	this->m_DataLength = this->m_Coefficients->GetBufferedRegion().GetSize();
    }

//     //JV input should be double or float. To keep it generic: cast to the CoefficientImageType
//     /** Dimension underlying input image. */
//     itkStaticConstMacro(ImageDimension, unsigned int, Superclass::ImageDimension);
//     /** Internal Coefficient typedef support */
//     DD( ImageDimension);
//     typedef Image<TCoefficientType,ImageDimension> CoefficientImageType;
//     typedef itk::CastImageFilter< TImageType, CoefficientImageType> CastFilterType;
//     typename CastFilterType::Pointer castFilter=CastFilterType::New();
//     castFilter->SetInput(inputData);
//     castFilter->Update();
//      DS this->InterpolateImageFunction<TImageType,TCoordRep>::m_Coefficients = inputData;

//     //JV cast to the coeff type, don't tell my mother
//     this->InterpolateImageFunction<TImageType,TCoordRep>::SetInputImage(inputData);
//     this->m_Coefficients=castFilter->GetOutput();
//     DS ??????    this->m_Coefficients = inputData;

//     //   //============================== old not generic solution
//     //      this->InterpolateImageFunction<TImageType,TCoordRep>::SetInputImage(inputData);
//          // DS this->InterpolateImageFunction<TImageType,TCoordRep>::m_Coefficients = inputData;
//     //     this->m_Coefficients = inputData;
//     //   //==============================    
//     if ( this->m_Coefficients.IsNotNull()) {
//       this->m_DataLength = this->m_Coefficients->GetBufferedRegion().GetSize();
//     }
  }

  //   t.Stop();  
  if (!inputData) return;
 //  mCoefficientTimer = t;

  // Compute Memory offset inside coefficients images (for looping over coefficients)
  static const int d = TImageType::ImageDimension;
  mInputMemoryOffset[0] = 1;
  for(int l=1; l<d; l++) {
    mInputMemoryOffset[l] = 
      mInputMemoryOffset[l-1]*this->m_Coefficients->GetLargestPossibleRegion().GetSize(l-1);
  }
  
  // Compute mSupportOffset according to input size
  mSupportOffset.resize(mSupportSize);
  mSupportIndex.resize(mSupportSize);
  for(int l=0; l<d; l++) mSupportIndex[0][l] = 0;
  for(unsigned int k=0; k<mSupportSize; k++) {
    // Get memory offset
    mSupportOffset[k] = Index2Offset<TImageType::ImageDimension>(mSupportIndex[k], mInputMemoryOffset);
    // next coefficient index
    if (k != mSupportSize-1) {
      for(int l=0; l<d; l++) mSupportIndex[k+1][l] = mSupportIndex[k][l];
      int l=0;
      bool stop = false;
      while (!stop) {
	mSupportIndex[k+1][l]++;
	if (mSupportIndex[k+1][l] == (int)mSupport[l]) { //ds supportindex=uint et support=int
	  mSupportIndex[k+1][l] = 0;
	  l++;
	}
	else stop = true;
      }
    }
  }

  // Check  
  for(int l=0; l<d; l++) {
    if (mOutputSpacing[l] == -1) {
      std::cerr << "Please use SetOutputSpacing before using bslut interpolator" << std::endl;
      exit(0);
    }
  }

  // Compute BSpline weights if not up to date
  if (!mWeightsAreUpToDate) UpdatePrecomputedWeights();

  // Initialisation
  *mIntrinsecErrorMax = *mIntrinsecError = 0.0;
  *mNumberOfError = 0;  
}
//====================================================================

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
void BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
UpdatePrecomputedWeights() {
//   mLUTTimer.Reset();
//   mLUTTimer.Start();
  mWeightsCalculator.SetSplineOrders(mSplineOrders);
  mWeightsCalculator.SetSamplingFactors(mSamplingFactors);
  mWeightsCalculator.ComputeTensorProducts();
  mWeightsAreUpToDate = true;
  //DS
  //   coef = new TCoefficientType[mSupportSize];   
  //     mCorrectedSupportIndex.resize(mSupportSize);
  //     mCorrectedSupportOffset.resize(mSupportSize);
 //  mLUTTimer.Stop();
//   mLUTTimer.Print("LUT      \t");
}
//====================================================================

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
typename BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::IndexType
BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
GetSampleIndexOfPixelPosition(const ContinuousIndexType & x, IndexType & EvaluateIndex) const {

  /*
    WARNING : sometimes, a floating number x could not really be
    represented in memory. In this case, the difference between x and
    floor(x) can be almost 1 (instead of 0).  So we take into account
    such special case, otherwise it could lead to annoying results.
  */
  // static const TCoefficientType tiny = 1.0e-7;
  IndexType index;

  for(int l=0; l<TImageType::ImageDimension; l++) {
  // bool mChange = false;

  // Compute t1 = distance to floor 
  TCoefficientType t1 = x[l]- vcl_floor(x[l]);
    
  // Compute index in precomputed weights table
  TCoefficientType t2 = mSamplingFactors[l]*t1;
  index[l] = (IndexValueType)lrint(t2);

  // For even order : test if too close to 0.5 (but lower). In this
  // case : take the next coefficient
  if (!(mSplineOrders[l] & 1)) {
    if (t1<0.5) {
      if (mSamplingFactors[l] & 1) {
	if (index[l] ==  (int) mSamplingFactors[l]/2+1) EvaluateIndex[l] = EvaluateIndex[l]+1;
      }
      else if (index[l] == (int) mSamplingFactors[l]/2) EvaluateIndex[l] = EvaluateIndex[l]+1;
    }
  }

  // Statistics (to be removed)
  /*
  *mIntrinsecError += fabs(index[l]-t2);		    
  (*mNumberOfError)++;
  if (fabs(index[l]-t2)> *mIntrinsecErrorMax) *mIntrinsecErrorMax = fabs(index[l]-t2);
  */

  // When to close to 1, take the next coefficient for ordd order, but
  // only change index for odd
    if (index[l] == (int)mSamplingFactors[l]) {
      index[l] = 0;
      if (mSplineOrders[l] & 1) EvaluateIndex[l] = EvaluateIndex[l]+1;
    }
  }

  // The end
  return index;
}
//====================================================================

//====================================================================
template <class TImageType, class TCoordRep, class TCoefficientType>
typename BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::OutputType
BSplineInterpolateImageFunctionWithLUT<TImageType,TCoordRep,TCoefficientType>::
EvaluateAtContinuousIndex(const ContinuousIndexType & x) const {

  // For shorter coding
  static const unsigned int d = TImageType::ImageDimension;

  // Compute the indexe of the first interpolation coefficient in the
  // coefficient image
  IndexType EvaluateIndex;
  long indx;
  for (unsigned int l=0; l<d; l++)  {
    if (mSplineOrders[l] & 1) {  // Use this index calculation for odd splineOrder (like cubic)
      indx = (long)vcl_floor(x[l]) - mSplineOrders[l] / 2 ; //this->m_SplineOrder / 2;
      EvaluateIndex[l] = indx;
    }
    else { // Use this index calculation for even splineOrder
      if (mSplineOrders[l] == 0) EvaluateIndex[l] = (long)rint(x[l]);
      else {
	indx = (long)vcl_floor((x[l]+ 0.5)) - mSplineOrders[l] / 2; //this->m_SplineOrder / 2;
	EvaluateIndex[l] = indx;
      }
    }
  }

  // Compute index of precomputed weights and get pointer to first weights
  const IndexType weightIndex = GetSampleIndexOfPixelPosition(x, EvaluateIndex);
  const TCoefficientType * pweights = mWeightsCalculator.GetFirstTensorProduct(weightIndex);

  // Check boundaries
  bool mBoundaryCase = false;
  for (unsigned int l=0; l<d; l++) {
    if ((EvaluateIndex[l] < 0) ||
	(EvaluateIndex[l]+mSupport[l]) >= this->m_Coefficients->GetLargestPossibleRegion().GetSize(l)) {
      mBoundaryCase = true;
    }
  }

  // Pointer to support offset
  const int * psupport;
    
  // Special case for boundary (to be changed ....)
  std::vector<int> mCorrectedSupportOffset;//(mSupportSize);
  if (mBoundaryCase) {
    //    return -1000;
    //std::vector<TCoefficientType> coef(mSupportSize);    
    // DD(EvaluateIndex);
     std::vector<IndexType> mCorrectedSupportIndex;//(mSupportSize);
    mCorrectedSupportIndex.resize(mSupportSize);
    mCorrectedSupportOffset.resize(mSupportSize);
    for(unsigned int i=0; i<mSupportSize; i++) {
      // DD(mSupportIndex[i]);
      for (unsigned int l=0; l<d; l++) {
	long a = mSupportIndex[i][l] + EvaluateIndex[l];
	long b = this->m_Coefficients->GetLargestPossibleRegion().GetSize(l);
	// DD(a);
	// 	DD(b);
	long d2 = 2 * b - 2;
	if (a < 0) {
	  mCorrectedSupportIndex[i][l] = -a - d2*(-a/d2) - EvaluateIndex[l];//mSupportIndex[i][l]-a;
	}
	else {
	  if (a>=b) {
	    mCorrectedSupportIndex[i][l] = d2 - a - EvaluateIndex[l];
	  }
	  else {
	    mCorrectedSupportIndex[i][l] = mSupportIndex[i][l]; //a - d2*(a/d2) - EvaluateIndex[l];
	  }
	  /*
	  if (a>=b) {
	    mCorrectedSupportIndex[i][l] = d2 - a - EvaluateIndex[l];//mSupportIndex[i][l] - (a-(b-1));
	  }
	  else {
	    mCorrectedSupportIndex[i][l] = mSupportIndex[i][l];
	  }
	  */
	}
      }
      // DD(mCorrectedSupportIndex[i]);
      mCorrectedSupportOffset[i] = Index2Offset<TImageType::ImageDimension>(mCorrectedSupportIndex[i], mInputMemoryOffset);
    }
    // for (unsigned int l=0; l<d; l++) {
    //       EvaluateIndex[l] = EvaluateIndex[l] + mCorrectedSupportIndex[0][l];
    //     }
    // DD(EvaluateIndex);
    psupport = &mCorrectedSupportOffset[0];
  }
  else {
    psupport = &mSupportOffset[0];
  }

  // Get pointer to first coefficient. Even if in some boundary cases,
  // EvaluateIndex is out of the coefficient image, 
  const TCoefficientType * pcoef = &(this->m_Coefficients->GetPixel(EvaluateIndex));

  // Main loop over BSpline support
  TCoefficientType interpolated = 0.0;
  for (unsigned int p=0; p<mSupportSize; p++) {
    interpolated += pcoef[*psupport] * (*pweights);
    ++psupport;
    ++pweights;
  }

  // Return interpolated value
  return(interpolated);    
}
//====================================================================

#endif