#ifndef __itkSignedMaurerParallelDistanceMapImageFilter_txx
#define __itkSignedMaurerParallelDistanceMapImageFilter_txx

#include "itkSignedMaurerParallelDistanceMapImageFilter.h"
#include "itkImageLinearConstIteratorWithIndex.h"
#include "itkImageLinearIteratorWithIndex.h"
#include "itkBinaryErodeImageFilter.h"
#include "itkAddImageFilter.h"
#include "itkBinaryBallStructuringElement.h"
#include "itkBinaryThresholdImageFilter.h"

#include "vnl/vnl_math.h"

/** Function to synchronize by waiting for all threads. */
namespace itk
{
template<class TInputImage, class TOutputImage>
void
SignedMaurerParallelDistanceMapImageFilter<TInputImage, TOutputImage>
::WaitForAll ()
{
  m_Barrier->Wait();
}

/** Allocate data, do conversion to values of 1 and 0, erode the image,
 *  sum the images, and do the distance transform. */
template<class TInputImage, class TOutputImage>
void
SignedMaurerParallelDistanceMapImageFilter<TInputImage, TOutputImage>
::GenerateData()
{
  // Allocate image.
  this->GetOutput()->SetRegions(this->GetInput()->GetRequestedRegion());
  this->GetOutput()->Allocate();

  int count;
  // Used for anisotropic image dimensions.
  m_SpacingSquared = this->GetOutput()->GetSpacing();
  for(count=0; count<InputImageDimension; count++)
    {
    m_SpacingSquared[count] = m_SpacingSquared[count] * m_SpacingSquared[count];
    }

  typedef BinaryThresholdImageFilter<InputImageType, InputImageType> 
    BinaryFilterType; 

  typename BinaryFilterType::Pointer binaryFilter = BinaryFilterType::New();
 
  if(m_BinaryConvert)
    {
    binaryFilter->SetLowerThreshold(m_ForegroundValue);
    binaryFilter->SetUpperThreshold(m_ForegroundValue);
    binaryFilter->SetInsideValue(1);
    binaryFilter->SetOutsideValue(0);
    binaryFilter->SetNumberOfThreads(this->GetNumberOfThreads());
    binaryFilter->SetInput(this->GetInput());
    }

  if(m_Signed)
    {
    typedef BinaryBallStructuringElement< 
      InputPixelType, 
      InputImageDimension  > StructuringElementType;  

    typedef BinaryErodeImageFilter< 
      InputImageType, 
      InputImageType, 
      StructuringElementType >     ErodeFilterType; 

    typename ErodeFilterType::Pointer binaryEroder = ErodeFilterType::New();

    // Erode the input image by 1.
    StructuringElementType structuringElement;
    structuringElement.SetRadius(1);
    structuringElement.CreateStructuringElement();
    binaryEroder->SetKernel(structuringElement);
    binaryEroder->SetErodeValue(m_ForegroundValue);
    binaryEroder->SetNumberOfThreads(this->GetNumberOfThreads());
    if(m_BinaryConvert)
      {
      binaryEroder->SetInput(binaryFilter->GetOutput());
      }
    else
      {
      binaryEroder->SetInput(this->GetInput());
      }

    typedef AddImageFilter<InputImageType,InputImageType,InputImageType> 
      AddFilterType;

    typename AddFilterType::Pointer sum = AddFilterType::New();

    // Add the eroded input image to the uneroded one.
    // This makes interior elements 2, edge elements 1, 
    // and exterior elements 0.
    sum->SetInput1(this->GetInput());
    sum->SetInput2(binaryEroder->GetOutput());
    sum->SetNumberOfThreads(this->GetNumberOfThreads());
    sum->Update();
  
    // Variable for this sum image used for signed calculations.
    m_BaseImage = sum->GetOutput();
    }
  else
    {
    // Variable for the input image used for unsigned calculations.
    if(m_BinaryConvert)
      {
      m_BaseImage = binaryFilter->GetOutput();
      }
    else
      {
      m_BaseImage = this->GetInput();
      }
    }

  // Class variable used particularly for indexing.
  m_Size = m_BaseImage->GetLargestPossibleRegion().GetSize();

  // Set up class variable to designate uninitialized positions
  for(int i=0; i<InputImageDimension; i++)
    {
    m_Uninitialized[i]=-1;
    }

  int totalSize=1;
  for(count=0; count<InputImageDimension; count++) 
    {
    totalSize*=m_Size[count];
    }

  m_ClosestExterior = 
    static_cast<InputIndexType*>
      (malloc(totalSize*sizeof(InputIndexType)));
  m_ClosestInterior = 
    static_cast<InputIndexType*>
      (malloc(totalSize*sizeof(InputIndexType)));

  if (m_ClosestExterior == NULL || m_ClosestInterior == NULL)
  {
    itkExceptionMacro( 
     << "Unable to allocate space for closest feature array." );
  }

  if(!m_Signed)
  {
    // Only one call is needed if interior distances are unnecessary
    ComputeDistanceTransform(m_ClosestExterior, 1, 0);
  }
  else
  {
    // Two calls are needed if interior distances are necessary
    ComputeDistanceTransform(m_ClosestExterior, 1, 0);

    ComputeDistanceTransform(m_ClosestInterior, 0, 1);
  }

  // Free matrices after completion of processing.
  free(m_ClosestExterior);
  free(m_ClosestInterior);
}

/** Split the requested region for the specified thread on the specified
 *  dimension. Based on SplitRequestedRegion from the ImageSource class. */
template <class TInputImage, class TOutputImage>
unsigned int
SignedMaurerParallelDistanceMapImageFilter<TInputImage, TOutputImage>
::SplitRequestedRegionOnNPlane(unsigned int i, unsigned int num, 
                               OutputImageRegionType& splitRegion,
                               unsigned int dimension)
{
  // Get the output pointer.
  OutputImageType * outputPtr = this->GetOutput();
  const typename TOutputImage::SizeType& requestedRegionSize
    = outputPtr->GetRequestedRegion().GetSize();

  unsigned int splitAxis=dimension;
  typename TOutputImage::IndexType splitIndex;
  typename TOutputImage::SizeType splitSize;

  // Initialize the splitRegion to the output requested region.
  splitRegion = outputPtr->GetRequestedRegion();
  splitIndex = splitRegion.GetIndex();
  splitSize = splitRegion.GetSize();

  // Determine the actual number of pieces that will be generated.
  typename TOutputImage::SizeType::SizeValueType range = 
    requestedRegionSize[splitAxis];
  unsigned int valuesPerThread = 
    (unsigned int)::ceil(range/(double)num);
  unsigned int maxThreadIdUsed = 
    (unsigned int)::ceil(range/(double)valuesPerThread) - 1;

  // Split the region.
  if (i < maxThreadIdUsed)
    {
    splitIndex[splitAxis] += i*valuesPerThread;
    splitSize[splitAxis] = valuesPerThread;
    }
  if (i == maxThreadIdUsed)
    {
    splitIndex[splitAxis] += i*valuesPerThread;
    // The last thread needs to process the rest of the dimension being split.
    splitSize[splitAxis] = splitSize[splitAxis] - i*valuesPerThread;
    }

  // Set the split region index and size variables.
  splitRegion.SetIndex( splitIndex );
  splitRegion.SetSize( splitSize );

  itkDebugMacro("  Split Piece: " << splitRegion );

  return maxThreadIdUsed + 1;
}

/** Start the multithreaded distance transform. */
template<class TInputImage, class TOutputImage>
void
SignedMaurerParallelDistanceMapImageFilter< TInputImage, TOutputImage >
::ComputeDistanceTransform(InputIndexType *closest, 
                           InputPixelType featureVal, bool writeOutput)
{
  // Set thread struct values from parameters.
  MaurerThreadStruct str;
  str.Filter = this;
  str.Closest = closest;
  str.Write = writeOutput;
  str.Feature = featureVal;

  // Set up multithreading.
  this->GetMultiThreader()->SetNumberOfThreads(this->GetNumberOfThreads());
  m_Barrier->Initialize(this->GetMultiThreader()->GetNumberOfThreads());
  this->GetMultiThreader()->
    SetSingleMethod(this->ComputeDistanceTransformThreaderCallback, &str);

  // Start multithreading.
  this->GetMultiThreader()->SingleMethodExecute();
}

/** Set up thread information and region split and procede with 
 *  the threaded distance transform. */
template<class TInputImage, class TOutputImage>
ITK_THREAD_RETURN_TYPE
SignedMaurerParallelDistanceMapImageFilter<TInputImage, TOutputImage>
::ComputeDistanceTransformThreaderCallback( void * arg )
{
  MaurerThreadStruct *str;

  unsigned int totalN;
  unsigned int totalX;
  unsigned int threadID;
  unsigned int threadCount;

  // Pull information from threading struct.
  threadID = ((MultiThreader::ThreadInfoStruct *)(arg))->ThreadID;
  threadCount = ((MultiThreader::ThreadInfoStruct *)(arg))->NumberOfThreads;

  str = (MaurerThreadStruct *)
          (((MultiThreader::ThreadInfoStruct *)(arg))->UserData);

  OutputImageRegionType splitRegionNPlane;
  totalN = str->Filter->
    SplitRequestedRegionOnNPlane(threadID, threadCount, splitRegionNPlane, InputImageDimension-1);

  OutputImageRegionType splitRegionXPlane;
  totalX = str->Filter->
    SplitRequestedRegionOnNPlane(threadID, threadCount, splitRegionXPlane, 0);

  // Call threaded function.
  str->Filter->
    ThreadedComputeDistanceTransform(splitRegionNPlane, splitRegionXPlane, 
                                     threadID, totalN, totalX, str->Closest, 
                                     str->Feature, str->Write);

  return ITK_THREAD_RETURN_VALUE;
}

/** Find the closest features in the dimensions less than the highest, then the highest, and do the 
 *  final distance transform calculation. Synchronize between the 
 *  lower-dimension stage and the highest-dimension one. */
template< class TInputImage, class TOutputImage >
void
SignedMaurerParallelDistanceMapImageFilter< TInputImage, TOutputImage >
::ThreadedComputeDistanceTransform(const OutputImageRegionType&
                                   outputNPlanesRegionForThread,
                                   const OutputImageRegionType&
                                   outputXPlanesRegionForThread,
                                   unsigned int threadID, unsigned int totalN,
                                   unsigned int totalX,
                                   InputIndexType *closest, 
                                   InputPixelType featureVal,
                                   bool writeOutput)
{
  // Only do if there are enough batches of Nth-dimension planes for this thread.
  if(threadID<totalN)
    {
    // Compute closest features in X dimension.
    distanceForXPlane(closest, featureVal,
                      outputNPlanesRegionForThread, threadID, writeOutput);

    // Loop through lower (non-X) dimensional distance transforms.
    for(int i=1; i<InputImageDimension-1; i++)
      {
      // Compute closest features in ith dimension.
      distanceForNPlane(closest, outputNPlanesRegionForThread, threadID, writeOutput, i);
      }
    }


  // Barrier to avoid conflicting accesses in the next step
  this->WaitForAll();


  // Only do if there are enough batches of X planes for this thread
  // and it was not taken care of earlier. 
  if(threadID<totalX && InputImageDimension>1)
    {
    // Compute closest features in highest dimension.
    distanceForNPlane(closest, outputXPlanesRegionForThread, 
                      threadID, writeOutput, InputImageDimension-1);
    }

}

/** Compute the closest features in X by looping through each X line 
 *  and updating each element's closest feature. */
template<class TInputImage, class TOutputImage>
void
SignedMaurerParallelDistanceMapImageFilter<TInputImage, TOutputImage>
::distanceForXPlane(InputIndexType *closest, InputPixelType featureVal, 
                    const OutputImageRegionType& threadOutputRegion, 
                    unsigned int threadID, bool writeOutput)
{
  int numOfFeatures;
  InputIndexType start = threadOutputRegion.GetIndex();
  InputSizeType size = threadOutputRegion.GetSize();

  int *xFeatures = static_cast<int*>(calloc(size[0],sizeof(int)));
  int numIndex;
  int initNumIndex;

  InputSizeType sizeMult;
  for(int i=0; i<InputImageDimension; i++)
    {
    sizeMult[i]=1;  
 
    for(int j=0; j<InputImageDimension; j++)
      {
      if(j<i)
        {
        sizeMult[i]*=m_Size[j];
        }
      } 
    }
    
  typedef ImageLinearConstIteratorWithIndex<InputImageType> ConstIteratorType;
  ConstIteratorType iter( m_BaseImage, threadOutputRegion );
  iter.SetDirection(0);
  iter.GoToBegin();

  typedef ImageLinearIteratorWithIndex<OutputImageType> IteratorType;
  IteratorType imageIter( this->GetOutput(), threadOutputRegion );
  imageIter.SetDirection(0);
  imageIter.GoToBegin();

  int x;
  // Iterate over the region.
  for(iter.GoToBegin(); !iter.IsAtEnd(); iter.NextLine()) 
    {
    numIndex=0; 
    InputIndexType lineIndex=iter.GetIndex();
    x=lineIndex[0];   
 
    for(int i=0; i<InputImageDimension; i++)
      {
      numIndex+=lineIndex[i]*sizeMult[i];
      }
    initNumIndex=numIndex;
       
    numOfFeatures = 0;

    // Iterate over the X line.
    while( !iter.IsAtEndOfLine() )
      {
      // Initialize the closest feature array.
      closest[numIndex] = m_Uninitialized; 
      InputPixelType meshVal = iter.Get();
       
      // Find features based on featureVal.
      if((m_Signed && meshVal >= featureVal && meshVal < (featureVal+2)) ||
         (!m_Signed && meshVal == featureVal))
        {
        xFeatures[numOfFeatures] = x;
        numOfFeatures++;
        }
      ++iter;
      numIndex++;
      x++; 
      }
     
    x = start[0];
    numIndex = initNumIndex; 
    iter.SetIndex(imageIter.GetIndex());

    // Assign closest feature values.
    for(int f(1); f <= numOfFeatures-1;++f)
      {
      double middle_x = (xFeatures[f-1] + xFeatures[f])/2;
      while(x <= middle_x)
        {
        InputIndexType v = lineIndex;
        v[0]=xFeatures[f-1];
        closest[numIndex] = v;

        // If this is the highest dimension, do the distance calculation.
        if(writeOutput && InputImageDimension == 1)
          {
          OutputPixelType square = 0;
          v=m_ClosestExterior[numIndex];

          OutputPixelType distance;
          if(m_UseImageSpacing)
            {
            distance = static_cast<OutputPixelType>(m_SpacingSquared[0]*(x-v[0])*(x-v[0]));
            }
          else
            {
            distance = (x-v[0])*(x-v[0]);
            }

          if(m_Signed)
            {
            v=m_ClosestInterior[numIndex];
            OutputPixelType signedDistance;
            if(m_UseImageSpacing)
              {
              signedDistance = static_cast<OutputPixelType>(m_SpacingSquared[0]*(x-v[0])*(x-v[0]));
              }
            else
              {
              signedDistance = (x-v[0])*(x-v[0]);
              }

            if(iter.Get() == 2)
              {
              if(m_SquaredDistance)
                {
                square = signedDistance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(signedDistance));
                }
              if(!m_InsideIsPositive)
                {
                square*=-1;
                }
              }
            else
              {
              if(m_SquaredDistance)
                {
                square = distance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(distance));
                }
              if(m_InsideIsPositive)
                {
                square*=-1;
                }
              }

            imageIter.Set(square);
            }
          else
            {
            if(m_SquaredDistance)
              {
              square = distance;
              }
            else
              {
              square = static_cast<OutputPixelType>(sqrt(distance));
              }

            imageIter.Set(square);
            }

          ++iter;
          ++imageIter;
          }

        numIndex++;
        x++;
        }
      }

    // Assign the last closest feature value.
    if(numOfFeatures > 0)
      {
      while (x < (start[0]+size[0]))
        {
        InputIndexType v = lineIndex;
        v[0]=xFeatures[numOfFeatures-1];
        closest[numIndex] = v;

        // If this is the highest dimension, do the distance calculation.
        if(writeOutput && InputImageDimension == 1)
          {
          OutputPixelType square = 0;
          v=m_ClosestExterior[numIndex];

          OutputPixelType distance;
          if(m_UseImageSpacing)
            {
            distance = static_cast<OutputPixelType>(m_SpacingSquared[0]*(x-v[0])*(x-v[0]));
            }
          else
            {
            distance = (x-v[0])*(x-v[0]);
            }

          if(m_Signed)
            {
            v=m_ClosestInterior[numIndex];

            OutputPixelType signedDistance;
            if(m_UseImageSpacing)
              {
              signedDistance = static_cast<OutputPixelType>(m_SpacingSquared[0]*(x-v[0])*(x-v[0]));
              }
            else
              {
              signedDistance = (x-v[0])*(x-v[0]);
              }

            if(iter.Get() == 2)
              {
              if(m_SquaredDistance)
                {
                square = signedDistance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(signedDistance));
                }
              if(!m_InsideIsPositive)
                {
                square*=-1;
                }
              }
            else
              {
              if(m_SquaredDistance)
                {
                square = distance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(distance));
                }

              if(m_InsideIsPositive)
                {
                square*=-1;
                }
              }

            imageIter.Set(square);
            }
          else
            {
            if(m_SquaredDistance)
              {
              square = distance;
              }
            else
              {
              square = static_cast<OutputPixelType>(sqrt(distance));
              }

            imageIter.Set(square);
            }

          ++iter;
          ++imageIter;
          }

        numIndex++;
        x++;
        }
      }
    x=0;
    }
    
  free(xFeatures);

}

/** Compute the closest features in N by looping through each N line and 
 *  updating each element's closest feature.  To avoid repeating loops, do 
 *  final distance calculation here as well. */
template<class TInputImage, class TOutputImage>
void
SignedMaurerParallelDistanceMapImageFilter<TInputImage, TOutputImage>
::distanceForNPlane(InputIndexType* closest, 
                    const OutputImageRegionType& threadOutputRegion, 
                    unsigned int threadID, bool writeOutput, int dim)
{
  InputIndexType thisClosest;
  int *thisStore=static_cast<int*>(calloc(dim+1,sizeof(int)));
 
  int numOfFeatures;
  double nMiddle(0);
  int previousFeature(0);

  typedef ImageLinearIteratorWithIndex<OutputImageType> IteratorType;
  IteratorType imageIter( this->GetOutput(), threadOutputRegion );
  imageIter.SetDirection(dim);

  typedef ImageLinearConstIteratorWithIndex<InputImageType> ConstIteratorType;
  ConstIteratorType meshIter( m_BaseImage, threadOutputRegion );
  meshIter.SetDirection(dim);

  InputIndexType start = threadOutputRegion.GetIndex();
  InputSizeType size = threadOutputRegion.GetSize();

  int **theFeatures = static_cast<int**>(malloc((dim+1)*sizeof(int *)));
  for(int i=0; i<=dim; i++)
    {
    theFeatures[i] = static_cast<int*>(calloc(size[dim],sizeof(int)));
    }
  
  double *thisLeft = 
    static_cast<double*>(
      calloc(size[dim],sizeof(double)));

  InputSizeType sizeMult;
  for(int i=0; i<InputImageDimension; i++)
    {
    sizeMult[i]=1;

    for(int j=0; j<InputImageDimension; j++)
      {
      if(j<i)
        {
        sizeMult[i]*=m_Size[j];
        }
      }
    }

  int numIndex, initNumIndex; 
  // Iterate over the region.
  for(imageIter.GoToBegin(), meshIter.GoToBegin();
      !imageIter.IsAtEnd(); imageIter.NextLine(), meshIter.NextLine()) 
    {
    numIndex=0;
    InputIndexType lineIndex=imageIter.GetIndex();

    for(int i=0; i<InputImageDimension; i++)
      {
      numIndex+=lineIndex[i]*sizeMult[i];
      }
    initNumIndex=numIndex;

    numOfFeatures=0;

    // Iterate over the N line.
    while( !imageIter.IsAtEndOfLine()) 
      {
      // Get the closest feature from the N plane.
      thisClosest = closest[numIndex]; 

      // If thisClosest is not empty.
      if(thisClosest != m_Uninitialized)
        {
        for(int i=0; i<=dim; i++)
          {
          thisStore[i] = thisClosest[i];
          } 

        previousFeature = numOfFeatures-1;
        nMiddle = 0;
          
        // Retain only valid features.
        while(previousFeature >= 0)
          {
          double a=0.0;
          double b;
          double c;
          if(m_UseImageSpacing)
            {
            for(int i=0; i<dim; i++) 
              {
              a +=
                  (double)(m_SpacingSquared[i] * 
                  (theFeatures[i][previousFeature] - thisStore[i]) * 
                  ((2.0*lineIndex[i]) - (theFeatures[i][previousFeature] + thisStore[i])));
              } 
            b = 2.0 * m_SpacingSquared[dim] * (thisStore[dim] - theFeatures[dim][previousFeature]);
            }
          else
            {
            for(int i=0; i<dim; i++)
              {
              a +=
                  (double)
                  ((theFeatures[i][previousFeature] - thisStore[i]) *     
                  ((2.0*lineIndex[i]) - (theFeatures[i][previousFeature] + thisStore[i])));
              }
            b = 2.0 * (thisStore[dim] - theFeatures[dim][previousFeature]);
            }
          c = (theFeatures[dim][previousFeature] + thisStore[dim])/2.0;

          nMiddle = (a/static_cast<double>(b)) + c;


          if(nMiddle <= thisLeft[previousFeature])
            {
            //  Get rid of the previous feature.
            numOfFeatures--;
            previousFeature--;
            }
          else
            {
            previousFeature = -1; // Stop checking
            }
          }

        // Add to features.
        if(nMiddle < (start[dim]+size[dim]))
          {
          for(int i=0; i<=dim; i++)
            {
            theFeatures[i][numOfFeatures] = thisStore[i];
            }
          thisLeft[numOfFeatures] = nMiddle;
          numOfFeatures++;
          }
        }

      numIndex+=sizeMult[dim];        
      ++imageIter;
      } 

    int n=start[dim];
    imageIter.SetIndex(meshIter.GetIndex());
    numIndex=initNumIndex;

    // Assign closest features or output.
    for (int f(1); f <= numOfFeatures-1; ++f) 
      {
      while(n <= thisLeft[f])
        {
        InputIndexType v = lineIndex;
        for(int i=0; i<=dim; i++)
          {
          v[i]=theFeatures[i][f-1];
          }
        closest[numIndex]=v;

        if(writeOutput && (InputImageDimension-1) == dim)
          {
          OutputPixelType square = 0;
          v=m_ClosestExterior[numIndex];

          OutputPixelType distance=(OutputPixelType)0;
          if(m_UseImageSpacing)
            {
            for(int i=0; i<dim; i++)
              {
              distance += static_cast<OutputPixelType>(m_SpacingSquared[i]*(lineIndex[i]-v[i])*(lineIndex[i]-v[i]));
              }
            distance += static_cast<OutputPixelType>(m_SpacingSquared[dim]*(n-v[dim])*(n-v[dim]));
            }
          else
            {
            for(int i=0; i<dim; i++)
              {
              distance += (lineIndex[i]-v[i])*(lineIndex[i]-v[i]);
              }
            distance += (n-v[dim])*(n-v[dim]);
            }

          if(m_Signed)
            {
            v=m_ClosestInterior[numIndex];
            OutputPixelType signedDistance=0.0;
            if(m_UseImageSpacing)
              {
              for(int i=0; i<dim; i++)
                {
                signedDistance += static_cast<OutputPixelType>(m_SpacingSquared[i]*(lineIndex[i]-v[i])*(lineIndex[i]-v[i]));
                }
              signedDistance += static_cast<OutputPixelType>(m_SpacingSquared[dim]*(n-v[dim])*(n-v[dim]));
              }
            else
              {
              for(int i=0; i<=dim; i++)
                {
                signedDistance += (lineIndex[i]-v[i])*(lineIndex[i]-v[i]);
                }
              signedDistance += (n-v[dim])*(n-v[dim]);
              }

            if(meshIter.Get() == 2)
              {
              if(m_SquaredDistance)
                {
                square = signedDistance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(signedDistance));
                }
              if(!m_InsideIsPositive)
                {
                square*=-1;
                }
              }
            else
              {
              if(m_SquaredDistance)
                {
                square = distance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(distance));
                }
              if(m_InsideIsPositive)
                {
                square*=-1;
                }
              }

            imageIter.Set(square);
            }
          else
            {
            if(m_SquaredDistance)
              {
              square = distance;
              }
            else
              {
              square = static_cast<OutputPixelType>(sqrt(distance));
              }

            imageIter.Set(square);
            }

          ++meshIter;
          ++imageIter;
          }
        n++; 
        numIndex+=sizeMult[dim]; 
        }
      }

    // Assign last closest feature or output.
    if(numOfFeatures > 0)
      {
      while(n < (size[dim]+start[dim]))
        {
        InputIndexType v = lineIndex;
        for(int i=0; i<=dim; i++)
          {
          v[i]=theFeatures[i][numOfFeatures-1];
          }
        closest[numIndex]=v;          

        if(writeOutput && (InputImageDimension-1) == dim)
          {
          OutputPixelType square = 0;
          v=m_ClosestExterior[numIndex];

          OutputPixelType distance=0.0;
          if(m_UseImageSpacing)
            {
            for(int i=0; i<dim; i++)
              {
              distance += static_cast<OutputPixelType>(m_SpacingSquared[i]*(lineIndex[i]-v[i])*(lineIndex[i]-v[i]));
              }
            distance += static_cast<OutputPixelType>(m_SpacingSquared[dim]*(n-v[dim])*(n-v[dim]));
            }
          else
            {
            for(int i=0; i<dim; i++)
              {
              distance += (lineIndex[i]-v[i])*(lineIndex[i]-v[i]);
              }
            distance += (n-v[dim])*(n-v[dim]);              
            }

          if(m_Signed)
            {
            v=m_ClosestInterior[numIndex];
                
            OutputPixelType signedDistance=0.0;
            if(m_UseImageSpacing)
              {
              for(int i=0; i<dim; i++)
                {
                signedDistance += static_cast<OutputPixelType>(m_SpacingSquared[i]*(lineIndex[i]-v[i])*(lineIndex[i]-v[i]));
                }
              signedDistance += static_cast<OutputPixelType>(m_SpacingSquared[dim]*(n-v[dim])*(n-v[dim]));
              }
            else
              {
              for(int i=0; i<dim; i++)
                {
                signedDistance += (lineIndex[i]-v[i])*(lineIndex[i]-v[i]);
                }
              signedDistance += (n-v[dim])*(n-v[dim]);
              }
               
            if(meshIter.Get() == 2)
              {
              if(m_SquaredDistance)
                {
                square = signedDistance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(signedDistance));
                }
              if(!m_InsideIsPositive)
                {
                square*=-1;
                }
              }
            else
              {
              if(m_SquaredDistance)
                {
                square = distance;
                }
              else
                {
                square = static_cast<OutputPixelType>(sqrt(distance));
                }

              if(m_InsideIsPositive)
                {
                square*=-1;
                }
              }

            imageIter.Set(square);
            }
          else
            {
            if(m_SquaredDistance)
              {
              square = distance;
              }
            else
              {
              square = static_cast<OutputPixelType>(sqrt(distance));
              }

            imageIter.Set(square);
            }

          ++meshIter;
          ++imageIter;
          }

        n++;
        numIndex+=sizeMult[dim];
        }
      }
    }
    

  free(thisStore);
  free(thisLeft);
 
  for(int i=0; i<=dim; i++)
    {
    free(theFeatures[i]);
    }
  free(theFeatures);
}

/**
 * Standard PrintSelf method
 */
template <class TInputImage, class TOutputImage>
void
SignedMaurerParallelDistanceMapImageFilter<TInputImage, TOutputImage>
::PrintSelf(
  std::ostream& os, 
  Indent indent) const
{
  Superclass::PrintSelf( os, indent );
  os << indent << "Foreground Value: " << m_ForegroundValue << std::endl;
  os << indent << "Spacing Squared: " << m_SpacingSquared << std::endl;
  os << indent << "Size: " << m_Size << std::endl;
  os << indent << "Inside Positive: " << m_InsideIsPositive << std::endl;
  os << indent << "Squared Distance: " << m_SquaredDistance << std::endl;
  os << indent << "Use Image Spacing: " << m_UseImageSpacing << std::endl;
  os << indent << "Convert to Binary Form: " << m_BinaryConvert << std::endl;
  os << indent << "Signed: " << m_Signed << std::endl;
  os << indent << "Barrier: " << m_Barrier << std::endl;
  os << indent << "Base Image: " << m_BaseImage << std::endl;
  os << indent << "Closest Interior Vortexes: " << m_ClosestInterior 
     << std::endl;
  os << indent << "Closest Exterior Vortexes: " << m_ClosestExterior 
     << std::endl;
}

} // End namespace itk.

#endif