/*
 * Copyright (c) ICG. All rights reserved.
 * See copyright.txt for more information.
 *
 * Institute for Computer Graphics and Vision
 * Graz, University of Technology / Austria
 *
 *
 * 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.
 *
 *
 * Project     : MIPItkProjects
 * Module      : Evaluation
 * Class       : $RCSfile $
 * Language    : C++
 * Description : 
 *
 * Author     : Martin Urschler
 * EMail      : urschler@icg.tu-graz.ac.at
 * Date       : $Date: $
 * Version    : $Revision:  $
 * Full Id    : $Id:  $
 *
 */
 
#ifndef __NONLINEAR_REGISTRATION_UTILITIES_H__
#define __NONLINEAR_REGISTRATION_UTILITIES_H__

#include "itkLinearInterpolateImageFunction.h"

#include "itkTimeProbesCollectorBase.h"
#include "itkImageLinearIteratorWithIndex.h"
#include "itkGaussianOperator.h"
#include "itkVectorExpandImageFilter.h"
#include "itkShrinkImageFilter.h"
#include "itkConstNeighborhoodIterator.h"
#include "itkCastImageFilter.h"
#include "itkSubtractImageFilter.h"

namespace NonlinearRegistrationUtilities
{


template< typename TRealImageType >
void lineBasedConvolution(
   const typename TRealImageType::Pointer& image,
   const typename TRealImageType::SizeType& imageSize,
   const unsigned int dim,
   const float *const operatorKernel,
   const unsigned int operatorSize,
   const unsigned int operatorRadius )
{
   // create a scanline buffer for scanlines over current dimension
   const unsigned int sizeOfScanlineBuffer = 
      imageSize[dim] + 2 * operatorRadius;
   float *scanlineBuffer = new float[sizeOfScanlineBuffer];
   float *resultBuffer   = new float[sizeOfScanlineBuffer];
   
   //std::cout << "size of scanline buffer: " << sizeOfScanlineBuffer << 
   //   std::endl;

   itk::ImageLinearIteratorWithIndex<TRealImageType> resultIter(
      image, image->GetLargestPossibleRegion() );
   resultIter.SetDirection(dim);
   
   for (resultIter.GoToBegin();!resultIter.IsAtEnd();resultIter.NextLine())
   {
      // initialize the scanline buffer
      resultIter.GoToBeginOfLine();
      
      // fill the left padding values
      unsigned int counter = 0;
      while (counter < operatorRadius)
      {
         scanlineBuffer[counter++] = resultIter.Get();
      }
      
      while (!resultIter.IsAtEndOfLine())
      {
         scanlineBuffer[counter++] = resultIter.Get();
         ++resultIter;
      }
      
      const unsigned int lastIndex = 
         sizeOfScanlineBuffer-operatorRadius-1;
      while (counter < sizeOfScanlineBuffer)
      {
         scanlineBuffer[counter++] = scanlineBuffer[lastIndex];
      }
      
      // initialize the result buffer
      std::fill( resultBuffer, &resultBuffer[sizeOfScanlineBuffer], 0.0f );         
      
      // now perform the convolution 
      for (unsigned int convStep=0; convStep<operatorSize; ++convStep)
      {
         const unsigned int indexOffset = convStep-operatorRadius; 
         
         for ( unsigned int bufferPos=operatorRadius; 
               bufferPos<(sizeOfScanlineBuffer-operatorRadius); 
               ++bufferPos )
         {
            resultBuffer[bufferPos] += operatorKernel[convStep] * 
               scanlineBuffer[bufferPos+indexOffset];
         }
      }
      
      // store the result in the result image
      resultIter.GoToBeginOfLine();
      counter = operatorRadius;
      while (!resultIter.IsAtEndOfLine())
      {
         resultIter.Set( resultBuffer[counter++] );
         ++resultIter;
      }
   }
   
   delete [] scanlineBuffer;
   delete [] resultBuffer;
}





template< typename TImageType >
typename TImageType::Pointer
efficientDiscreteGaussSmoothVolume( 
   const typename TImageType::Pointer& image,
   const double sigma_x,
   const double sigma_y, 
   const double sigma_z )
{
   const unsigned int IMAGE_DIMENSION = 
      itk::GetImageDimension<TImageType>::ImageDimension;
      
   std::cout << "Discrete gauss-smoothing sigmas in mm: " << sigma_x << " ; " << 
      sigma_y << " ; " << sigma_z << std::endl;
   
   const typename TImageType::SizeType imageSize =
      image->GetLargestPossibleRegion().GetSize();
   
   // create an intermediate result image
   ITK_IMAGE_ALLOCATE_MACRO_TN( FloatImageType, floatResult,
      TImageType, image );
   
   // initialize intermediate result image with input image
   ITK_IMAGE_COPY_MACRO_TN( TImageType, image, 
                            FloatImageType, floatResult );

   
   double variances[IMAGE_DIMENSION];
   variances[0] = vnl_math_sqr( sigma_x ); 
   variances[1] = vnl_math_sqr( sigma_y ); 
   variances[2] = vnl_math_sqr( sigma_z );
   
   typedef itk::GaussianOperator<float, IMAGE_DIMENSION> GaussOperatorType;
   
   // FIXXME: think some more about even and odd image sizes
   //const bool imageSizeOdd = ((imageSize[dim] % 2) == 1) ? true : false;
   
   for (unsigned int dim = 0; dim < IMAGE_DIMENSION; ++dim)
   {
      // Create a gauss operator of appropriate size
      GaussOperatorType gaussOperator;
   
      // Set up the operator for this dimension
      gaussOperator.SetDirection(dim);
   
      // convert the variance from physical units to pixels
      double s = image->GetSpacing()[dim];
      s = s*s;
      gaussOperator.SetVariance(variances[dim] / s);
   
      gaussOperator.SetMaximumKernelWidth(32);
      gaussOperator.SetMaximumError(0.01);
      gaussOperator.CreateDirectional();
         
      const unsigned int operatorSize = gaussOperator.GetSize()[dim];
      const unsigned int operatorRadius = gaussOperator.GetRadius(dim);
                  
      float *operatorKernel = new float[operatorSize];
         
      for (int i=0; i<operatorSize; ++i)
      {
         operatorKernel[i] = gaussOperator[i];
         std::cout << operatorKernel[i] << ";";
      }
      std::cout << std::endl;
      
      lineBasedConvolution<FloatImageType>( floatResult, imageSize, dim,
         operatorKernel, operatorSize, operatorRadius );

      delete [] operatorKernel;
   }
   
   // create final result image
   ITK_IMAGE_ALLOCATE_MACRO_TN( TImageType, result,
      FloatImageType, floatResult );
   
   // copy intermediate result to final result
   ITK_IMAGE_COPY_MACRO_TN( FloatImageType, floatResult, 
                            TImageType, result );

   return result;
   
   // use the SameType template here - we need a compile time if for this 
   // purpose -> see boost lib
   
//   if (SameType<TImageType, FloatImageType>::value == true)
//   {
//      //std::cout << "Optimization in progress: float image is returned!" << 
//      //   std::endl;
//      return floatResult;
//   }
//   else
//   {
//      // create final result image
//      ITK_IMAGE_ALLOCATE_MACRO_TN( TImageType, result,
//         FloatImageType, floatResult );
//      
//      // copy intermediate result to final result
//      ITK_IMAGE_COPY_MACRO_TN( FloatImageType, floatResult, 
//                               TImageType, result );
//   
//      return result;
//   }
}






template< typename TDeformationFieldType >
void
efficientInplaceDiscreteGaussSmoothDeformationField( 
   typename TDeformationFieldType::Pointer& defField, 
   const double sigma_x_voxel,
   const double sigma_y_voxel, 
   const double sigma_z_voxel )
{
   //std::cout << "Discrete gauss-smoothing sigmas in voxel: " << 
   //   sigma_x_voxel << " ; " << sigma_y_voxel << " ; " << sigma_z_voxel 
   //   << std::endl;
   
   const unsigned int Dimensions = 
      itk::GetImageDimension<UInt16ImageType>::ImageDimension;
      
   const typename TDeformationFieldType::SizeType defFieldSize =
      defField->GetLargestPossibleRegion().GetSize();
   
   double variances[Dimensions];
   variances[0] = vnl_math_sqr( sigma_x_voxel ); 
   variances[1] = vnl_math_sqr( sigma_y_voxel ); 
   variances[2] = vnl_math_sqr( sigma_z_voxel );
   
   typedef itk::GaussianOperator<float, Dimensions> GaussOperatorType;
   
   for (unsigned int dim = 0; dim < Dimensions; ++dim)
   {
      // Create a gauss operator of appropriate size
      GaussOperatorType gaussOperator;
   
      // Set up the operator for this dimension
      gaussOperator.SetDirection(dim);
      gaussOperator.SetVariance(variances[dim]);
      gaussOperator.SetMaximumKernelWidth(32);
      gaussOperator.SetMaximumError(0.01);
      gaussOperator.CreateDirectional();
         
      const unsigned int operatorSize = gaussOperator.GetSize()[dim];
      const unsigned int operatorRadius = gaussOperator.GetRadius(dim);
                  
      float *operatorKernel = new float[operatorSize];
         
      for (unsigned int i=0; i<operatorSize; ++i)
      {
         operatorKernel[i] = gaussOperator[i];
         //std::cout << operatorKernel[i] << ";";
      }
      //std::cout << std::endl;
      
      
      // create a scanline buffer for scanlines over current dimension
      const unsigned int sizeOfScanlineBuffer = 
         defFieldSize[dim] + 2 * operatorRadius;
      float *scanlineBufferDefX = new float[sizeOfScanlineBuffer];
      float *resultBufferDefX   = new float[sizeOfScanlineBuffer];
      float *scanlineBufferDefY = new float[sizeOfScanlineBuffer];
      float *resultBufferDefY   = new float[sizeOfScanlineBuffer];
      float *scanlineBufferDefZ = new float[sizeOfScanlineBuffer];
      float *resultBufferDefZ   = new float[sizeOfScanlineBuffer];

      itk::ImageLinearIteratorWithIndex<TDeformationFieldType> defFieldIter(
         defField, defField->GetLargestPossibleRegion() );
      defFieldIter.SetDirection(dim);
      
      for (defFieldIter.GoToBegin();!defFieldIter.IsAtEnd();
           defFieldIter.NextLine())
      {
         // initialize the scanline buffer
         defFieldIter.GoToBeginOfLine();
         
         // fill the left padding values
         unsigned int counter = 0;
         VectorPixelType def = defFieldIter.Get();
         while (counter < operatorRadius)
         {
            scanlineBufferDefX[counter] = def[0];
            scanlineBufferDefY[counter] = def[1];
            scanlineBufferDefZ[counter] = def[2];
            ++counter;
         }
         
         while (!defFieldIter.IsAtEndOfLine())
         {
            def = defFieldIter.Get();
            scanlineBufferDefX[counter] = def[0];
            scanlineBufferDefY[counter] = def[1];
            scanlineBufferDefZ[counter] = def[2];
            ++counter;
            ++defFieldIter;
         }
         
         const unsigned int lastIndex = 
            sizeOfScanlineBuffer-operatorRadius-1;
         while (counter < sizeOfScanlineBuffer)
         {
            scanlineBufferDefX[counter] = scanlineBufferDefX[lastIndex];
            scanlineBufferDefY[counter] = scanlineBufferDefY[lastIndex];
            scanlineBufferDefZ[counter] = scanlineBufferDefZ[lastIndex];
            ++counter;
         }
         
         // initialize the result buffer
         std::fill( resultBufferDefX, 
            &resultBufferDefX[sizeOfScanlineBuffer], 0.0f );
         std::fill( resultBufferDefY, 
            &resultBufferDefY[sizeOfScanlineBuffer], 0.0f );
         std::fill( resultBufferDefZ, 
            &resultBufferDefZ[sizeOfScanlineBuffer], 0.0f );
            
         // now perform the convolution 
         for (unsigned int convStep=0; convStep<operatorSize; ++convStep)
         {
            const unsigned int indexOffset = convStep-operatorRadius; 
            
            for ( unsigned int bufferPos=operatorRadius; 
                  bufferPos<(sizeOfScanlineBuffer-operatorRadius); 
                  ++bufferPos )
            {
               resultBufferDefX[bufferPos] += operatorKernel[convStep] * 
                  scanlineBufferDefX[bufferPos+indexOffset];
               resultBufferDefY[bufferPos] += operatorKernel[convStep] * 
                  scanlineBufferDefY[bufferPos+indexOffset];
               resultBufferDefZ[bufferPos] += operatorKernel[convStep] * 
                  scanlineBufferDefZ[bufferPos+indexOffset];
            }
         }
         
         // store the convolution result back
         defFieldIter.GoToBeginOfLine();
         counter = operatorRadius;
         while (!defFieldIter.IsAtEndOfLine())
         {
            def[0] = resultBufferDefX[counter];
            def[1] = resultBufferDefY[counter];
            def[2] = resultBufferDefZ[counter];
            defFieldIter.Set( def );
            ++counter;
            ++defFieldIter;
         }
      }
      
      delete [] scanlineBufferDefX;
      delete [] resultBufferDefX;
      delete [] scanlineBufferDefY;
      delete [] resultBufferDefY;
      delete [] scanlineBufferDefZ;
      delete [] resultBufferDefZ;
      delete [] operatorKernel;
   }
}


template< typename TImageType >
typename TImageType::Pointer shrinkImageToHalf( 
   const typename TImageType::Pointer image )
{
   typename TImageType::SizeType originalImageSize =
      image->GetLargestPossibleRegion().GetSize();
   typename TImageType::SpacingType originalImageSpacing =
      image->GetSpacing();
   
   std::cout << "originalImageSize: " << originalImageSize << std::endl;
   std::cout << "originalImageSpacing: " << originalImageSpacing << std::endl;
   
   //typename TImageType::SizeType shrinkedImageSize;
   //for (int dim=0; dim<_DIMENSION_; ++dim)
   //{
   //   shrinkedImageSize[dim] = ceil( originalImageSize[dim] / 2 );
   //}
   
   //std::cout << "shrinkedImageSize: " << shrinkedImageSize << std::endl;
   
   const double sigma = 0.75 * vnl_math_min(originalImageSpacing[0],
      (vnl_math_min(originalImageSpacing[1], originalImageSpacing[2])));
   //std::cout << "sigma in mm: " << sigma << std::endl;
   
   itk::TimeProbesCollectorBase coll;
   coll.Start( "smooth" );
   
   // smooth the original image
   
   //typename TImageType::Pointer smoothedImage = 
   //   efficientDiscreteGaussSmoothVolume<TImageType>( image, 
   //      sigma, sigma, sigma );
      
   typename TImageType::Pointer smoothedImage =
      SmallUtilityMethods<TImageType>::gaussSmoothVolume(
         image, sigma, sigma, sigma );
   
   coll.Stop( "smooth" );
   coll.Report();
   
   typedef itk::ShrinkImageFilter<TImageType,TImageType> ShrinkerType;
   typename ShrinkerType::Pointer shrinker = ShrinkerType::New();
   shrinker->SetInput( smoothedImage );

   shrinker->SetShrinkFactor( 0, 2 );
   shrinker->SetShrinkFactor( 1, 2 );
   shrinker->SetShrinkFactor( 2, 2 );
   
   shrinker->Update();
   
   return shrinker->GetOutput();
}


template< typename TImageType >
typename TImageType::Pointer shrinkImageToHalfByAveraging( 
   const typename TImageType::Pointer image )
{
   typename TImageType::SizeType originalImageSize =
      image->GetLargestPossibleRegion().GetSize();
   typename TImageType::SpacingType originalImageSpacing =
      image->GetSpacing();

   std::cout << "originalImageSize: " << originalImageSize << std::endl;
   std::cout << "originalImageSpacing: " << originalImageSpacing << std::endl;
   
   typename TImageType::SizeType shrinkedImageSize = originalImageSize;
   shrinkedImageSize[0] /= 2;
   shrinkedImageSize[1] /= 2;
   shrinkedImageSize[2] /= 2;
   
   typename TImageType::SpacingType shrinkedImageSpacing = originalImageSpacing;
   shrinkedImageSpacing[0] *= 2.0;
   shrinkedImageSpacing[1] *= 2.0;
   shrinkedImageSpacing[2] *= 2.0;
   
   std::cout << "shrinkedImageSize: " << shrinkedImageSize << std::endl;
   std::cout << "shrinkedImageSpacing: " << shrinkedImageSpacing << std::endl;
   
   typename TImageType::RegionType shrinkedImageRegion = 
      image->GetLargestPossibleRegion();
   shrinkedImageRegion.SetSize( shrinkedImageSize );
   
   typename TImageType::Pointer shrinkedImage = TImageType::New(); 
   shrinkedImage->SetRegions( shrinkedImageRegion );
   shrinkedImage->SetSpacing( shrinkedImageSpacing ); 
   shrinkedImage->SetOrigin( image->GetOrigin() ); 
   shrinkedImage->Allocate(); 
   
   typename TImageType::IndexType shrinkedImageIndex;
   
   typename TImageType::IndexType origIndex0;
   typename TImageType::IndexType origIndex1;
   typename TImageType::IndexType origIndex2;
   typename TImageType::IndexType origIndex3;
   typename TImageType::IndexType origIndex4;
   typename TImageType::IndexType origIndex5;
   typename TImageType::IndexType origIndex6;
   typename TImageType::IndexType origIndex7;
   
   double average = 0.0;
   
   for (unsigned int z=0; z<shrinkedImageSize[2]; ++z)
   {
      shrinkedImageIndex[2] = z;
      
      for (unsigned int y=0; y<shrinkedImageSize[1]; ++y)
      {
         shrinkedImageIndex[1] = y;
               
         for (unsigned int x=0; x<shrinkedImageSize[0]; ++x)
         {
            shrinkedImageIndex[0] = x;
            
            origIndex0[0] = 2*shrinkedImageIndex[0];
            origIndex0[1] = 2*shrinkedImageIndex[1];
            origIndex0[2] = 2*shrinkedImageIndex[2];
                        
            origIndex1[0] = 2*shrinkedImageIndex[0]+1;
            origIndex1[1] = 2*shrinkedImageIndex[1];
            origIndex1[2] = 2*shrinkedImageIndex[2];
            
            origIndex2[0] = 2*shrinkedImageIndex[0];
            origIndex2[1] = 2*shrinkedImageIndex[1]+1;
            origIndex2[2] = 2*shrinkedImageIndex[2];
            
            origIndex3[0] = 2*shrinkedImageIndex[0];
            origIndex3[1] = 2*shrinkedImageIndex[1];
            origIndex3[2] = 2*shrinkedImageIndex[2]+1;
            
            origIndex4[0] = 2*shrinkedImageIndex[0]+1;
            origIndex4[1] = 2*shrinkedImageIndex[1]+1;
            origIndex4[2] = 2*shrinkedImageIndex[2];

            origIndex5[0] = 2*shrinkedImageIndex[0]+1;
            origIndex5[1] = 2*shrinkedImageIndex[1];
            origIndex5[2] = 2*shrinkedImageIndex[2]+1;
            
            origIndex6[0] = 2*shrinkedImageIndex[0];
            origIndex6[1] = 2*shrinkedImageIndex[1]+1;
            origIndex6[2] = 2*shrinkedImageIndex[2]+1;
            
            origIndex7[0] = 2*shrinkedImageIndex[0]+1;
            origIndex7[1] = 2*shrinkedImageIndex[1]+1;
            origIndex7[2] = 2*shrinkedImageIndex[2]+1;
            
            average = 
               image->GetPixel(origIndex0) + image->GetPixel(origIndex1) +
               image->GetPixel(origIndex2) + image->GetPixel(origIndex3) +
               image->GetPixel(origIndex4) + image->GetPixel(origIndex5) +
               image->GetPixel(origIndex6) + image->GetPixel(origIndex7);
            
            average /= 8.0;
            
            shrinkedImage->SetPixel( shrinkedImageIndex, 
               static_cast<typename TImageType::PixelType>(average) );
         }
      }
   }
   
   return shrinkedImage;
}


template< typename TImageType >
typename TImageType::Pointer upsampleImageByFactor2( 
   const typename TImageType::Pointer image )
{
   typename TImageType::SizeType originalImageSize =
      image->GetLargestPossibleRegion().GetSize();
   typename TImageType::SpacingType originalImageSpacing =
      image->GetSpacing();

   std::cout << "originalImageSize: " << originalImageSize << std::endl;
   std::cout << "originalImageSpacing: " << originalImageSpacing << std::endl;
   
   typename TImageType::SizeType upsampledImageSize = originalImageSize;
   upsampledImageSize[0] *= 2;
   upsampledImageSize[1] *= 2;
   upsampledImageSize[2] *= 2;
   
   typename TImageType::SpacingType upsampledImageSpacing = 
      originalImageSpacing;
   upsampledImageSpacing[0] *= 0.5;
   upsampledImageSpacing[1] *= 0.5;
   upsampledImageSpacing[2] *= 0.5;
   
   std::cout << "upsampledImageSize: " << upsampledImageSize << std::endl;
   std::cout << "upsampledImageSpacing: " << upsampledImageSpacing << std::endl;
   
   typename TImageType::RegionType upsampledImageRegion = 
      image->GetLargestPossibleRegion();
   upsampledImageRegion.SetSize( upsampledImageSize );
   
   typename TImageType::Pointer upsampledImage = TImageType::New(); 
   upsampledImage->SetRegions( upsampledImageRegion );
   upsampledImage->SetSpacing( upsampledImageSpacing ); 
   upsampledImage->SetOrigin( image->GetOrigin() ); 
   upsampledImage->Allocate(); 
   
   typename TImageType::IndexType loResIndex;
   typename TImageType::IndexType hiResIndex;
   
   for (unsigned int z=0; z<upsampledImageSize[2]; ++z)
   {  
      hiResIndex[2] = z;
      
      for (unsigned int y=0; y<upsampledImageSize[1]; ++y)
      {
         hiResIndex[1] = y;
         
         for (unsigned int x=0; x<upsampledImageSize[0]; ++x)
         {
            hiResIndex[0] = x;
              
            loResIndex[0] = x / 2;
            loResIndex[1] = y / 2;
            loResIndex[2] = z / 2;
            
            upsampledImage->SetPixel(hiResIndex, image->GetPixel(loResIndex));
         }
      }
   }
   
   return upsampledImage;
}



template<typename TDeformationFieldType>
void clearDeformationField( 
   typename TDeformationFieldType::Pointer& deformationField )
{
   const unsigned int Dimensions =
      itk::GetImageDimension<TDeformationFieldType>::ImageDimension;
      
   typename TDeformationFieldType::PixelType zeroDeformation =
      VectorPixelConstants<VectorPixelType::ValueType, Dimensions>::zero();
   
   deformationField->FillBuffer( zeroDeformation );
}



template< typename TNewDeformationFieldType, typename TOldDeformationFieldType >
void upsampleDeformationFieldByFactors2( 
         typename TNewDeformationFieldType::Pointer& new_deformation_field, 
   const typename TOldDeformationFieldType::Pointer& old_deformation_field )
{
   const unsigned int Dimensions =
      itk::GetImageDimension<TOldDeformationFieldType>::ImageDimension;

   typename TNewDeformationFieldType::SizeType new_size = 
      new_deformation_field->GetLargestPossibleRegion().GetSize();

   typedef typename itk::VectorExpandImageFilter< 
      TOldDeformationFieldType, TNewDeformationFieldType > UpsamplerType;
   
   typename UpsamplerType::Pointer upsampler = UpsamplerType::New();
   upsampler->SetInput( old_deformation_field );
   
   unsigned int expand_factors[Dimensions] = { 2, 2, 2 };
   upsampler->SetExpandFactors( expand_factors );

   upsampler->Update();
   
   new_deformation_field = upsampler->GetOutput();
   
   if (new_deformation_field->GetLargestPossibleRegion().GetSize() != new_size)
   {
      std::cout << "Error in upsampledDeformationFieldByFactors2: " <<
         "wrong size of upsampled field!" << std::endl;
   }
}


template< typename TNewDeformationFieldType, typename TOldDeformationFieldType >
void upsampleDeformationField( 
         typename TNewDeformationFieldType::Pointer& new_deformation_field, 
   const typename TOldDeformationFieldType::Pointer& old_deformation_field )
{
   const unsigned int Dimensions = 
      itk::GetImageDimension<TOldDeformationFieldType>::ImageDimension;

   typename TNewDeformationFieldType::SizeType new_size = 
      new_deformation_field->GetLargestPossibleRegion().GetSize();

   typename TOldDeformationFieldType::SizeType orig_size = 
      old_deformation_field->GetLargestPossibleRegion().GetSize();

   //MY_DEBUG_MACRO( "Upsampling deformation field." );
   //std::cout << "original deformation field size: " << orig_size << std::endl;
   //std::cout << "upsampled deformation field size: " << new_size << std::endl;

   // some typedefs for the iterators
   typedef typename itk::ImageRegionIteratorWithIndex<
      TNewDeformationFieldType> OutputItorType;

   OutputItorType outItor( new_deformation_field,
                           new_deformation_field->GetLargestPossibleRegion() );


   double upsample_factors[Dimensions];
   upsample_factors[0] = static_cast<double>(new_size[0]) / 
                         static_cast<double>(orig_size[0]);
   upsample_factors[1] = static_cast<double>(new_size[1]) / 
                         static_cast<double>(orig_size[1]);
   upsample_factors[2] = static_cast<double>(new_size[2]) / 
                         static_cast<double>(orig_size[2]);

   //std::cout << "upsample factors: " << upsample_factors[0] << " " << 
   //   upsample_factors[1] << " " << upsample_factors[2] << std::endl;

   const unsigned int nr_neighbors = (1 << Dimensions);

   for ( outItor.GoToBegin(); !outItor.IsAtEnd(); ++outItor )  
   {
      double resampledIndex[Dimensions];
      {
         typename TNewDeformationFieldType::IndexType outIndex = 
            outItor.GetIndex();

         // calculate resampled index and at the same time check if the 
         // resampled index is out of bounds
         resampledIndex[0] = 
            vnl_math_min( (static_cast<double>(outIndex[0]) / upsample_factors[0]), 
               static_cast<double>(orig_size[0]-1) );
         resampledIndex[1] = 
            vnl_math_min( (static_cast<double>(outIndex[1]) / upsample_factors[1]), 
               static_cast<double>(orig_size[1]-1) );
         resampledIndex[2] = 
            vnl_math_min( (static_cast<double>(outIndex[2]) / upsample_factors[2]), 
               static_cast<double>(orig_size[2]-1) );
      }

      // nearest neighbor FIXXME erroneous implementation, no boundary checking!
      //typename TOldDeformationFieldType::IndexType interpolatedIndex;
      //interpolatedIndex[0] = ROUND_DBL<int>( resampledIndex[0] );
      //interpolatedIndex[1] = ROUND_DBL<int>( resampledIndex[1] );
      //interpolatedIndex[2] = ROUND_DBL<int>( resampledIndex[2] );

      //outItor.Set( old_deformation_field->GetPixel( interpolatedIndex ) );


      

      // linear interpolate
      //std::cout << resampledIndex[0] << " " << resampledIndex[1] << " " << 
      //   resampledIndex[2] << std::endl;

      unsigned int dim;
      
      typename TNewDeformationFieldType::PixelType output;

      // compute base index as closest index below point
      // compute distance from point to base index
      signed long baseIndex[Dimensions];
      double distance[Dimensions];

      for (dim = 0; dim<Dimensions; ++dim)
      {
         baseIndex[dim] = static_cast<long>(floor( resampledIndex[dim] ));
         distance[dim] = resampledIndex[dim] - static_cast<double>( 
            baseIndex[dim] );

         output[dim] = 0.0;
      }

      // interpolated value is the weighted sum of the surrounding neighbours
      // the weight is the fractional overlap of the neighbor pixels with 
      // respect to a pixel centered on the point
      
      double totalOverlap = 0.0;
      for (unsigned int counter = 0; counter < nr_neighbors; ++counter)
      {
         double overlap = 1.0;
         unsigned int upper = counter;
         
         typename TOldDeformationFieldType::IndexType neighborIndex;

         for (dim = 0; dim<Dimensions; ++dim)
         {
            if (upper & 1)
            {
               neighborIndex[dim] = baseIndex[dim] + 1;
               overlap *= distance[dim];
            }
            else
            {
               neighborIndex[dim] = baseIndex[dim];
               overlap *= 1.0 - distance[dim];
            }

            upper >>= 1;
         }

         if (overlap)
         {
            //std::cout << "   " << neighborIndex << std::endl;

            const typename TOldDeformationFieldType::PixelType input = 
               old_deformation_field->GetPixel( neighborIndex );

            for(unsigned int k = 0; k < Dimensions; ++k )
            {
               output[k] += overlap * static_cast<double>( input[k] );
            }

            totalOverlap += overlap;
         }

         if (totalOverlap == 1.0)   
         {
            break;
         }
      }

      outItor.Set( output );
   }
}

} // end namespace



#endif // __NONLINEAR_REGISTRATION_UTILITIES_H__