/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
#ifndef itkMultiphaseSparseFiniteDifferenceImageFilter_hxx
#define itkMultiphaseSparseFiniteDifferenceImageFilter_hxx

#include "itkMultiphaseSparseFiniteDifferenceImageFilter.h"

namespace itk
{
template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
double MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  m_ConstantGradientValue = 1.0;

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
const typename MultiphaseSparseFiniteDifferenceImageFilter<
  TInputImage,
  TFeatureImage,
  TOutputImage,
  TFunction,
  TIdCell>::ValueType MultiphaseSparseFiniteDifferenceImageFilter<TInputImage,
                                                                  TFeatureImage,
                                                                  TOutputImage,
                                                                  TFunction,
                                                                  TIdCell>::m_ValueOne =
  NumericTraits<
    typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
      ValueType>::OneValue();

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
const typename MultiphaseSparseFiniteDifferenceImageFilter<
  TInputImage,
  TFeatureImage,
  TOutputImage,
  TFunction,
  TIdCell>::ValueType MultiphaseSparseFiniteDifferenceImageFilter<TInputImage,
                                                                  TFeatureImage,
                                                                  TOutputImage,
                                                                  TFunction,
                                                                  TIdCell>::m_ValueZero =
  NumericTraits<
    typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
      ValueType>::ZeroValue();

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
const typename MultiphaseSparseFiniteDifferenceImageFilter<
  TInputImage,
  TFeatureImage,
  TOutputImage,
  TFunction,
  TIdCell>::StatusType MultiphaseSparseFiniteDifferenceImageFilter<TInputImage,
                                                                   TFeatureImage,
                                                                   TOutputImage,
                                                                   TFunction,
                                                                   TIdCell>::m_StatusNull =
  NumericTraits<
    typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
      StatusType>::NonpositiveMin();

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
const typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage,
                                                           TFeatureImage,
                                                           TOutputImage,
                                                           TFunction,
                                                           TIdCell>::StatusType
  MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
    m_StatusChanging = -1;

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
const typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage,
                                                           TFeatureImage,
                                                           TOutputImage,
                                                           TFunction,
                                                           TIdCell>::StatusType
  MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
    m_StatusActiveChangingUp = -2;

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
const typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage,
                                                           TFeatureImage,
                                                           TOutputImage,
                                                           TFunction,
                                                           TIdCell>::StatusType
  MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
    m_StatusActiveChangingDown = -3;

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
const typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage,
                                                           TFeatureImage,
                                                           TOutputImage,
                                                           TFunction,
                                                           TIdCell>::StatusType
  MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
    m_StatusBoundaryPixel = -4;

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  MultiphaseSparseFiniteDifferenceImageFilter()
{
  this->m_CurrentFunctionIndex = 0;
  this->m_IsoSurfaceValue = m_ValueZero;
  this->m_BackgroundValue = NumericTraits<ValueType>::max();
  this->m_NumberOfLayers = ImageDimension;
  this->m_InterpolateSurfaceLocation = true;
  this->m_BoundsCheckingActive = false;
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  CopyInputToOutput()
{
  for (IdCellType i = 0; i < this->m_FunctionCount; i++)
  {
    InputImagePointer input = this->m_LevelSet[i];

    // This is used as a temporary buffer
    InputImagePointer tempImage = InputImageType::New();
    tempImage->SetRegions(input->GetRequestedRegion());
    tempImage->CopyInformation(input);
    tempImage->Allocate();

    // Compute Heaviside of input image
    // Copy input to temp
    InputRegionType                     region = input->GetRequestedRegion();
    ImageRegionIterator<InputImageType> lIt(input, region);
    ImageRegionIterator<InputImageType> tIt(tempImage, region);

    lIt.GoToBegin();
    tIt.GoToBegin();

    while (!lIt.IsAtEnd())
    {
      tIt.Set(lIt.Get());
      ++tIt;
      ++lIt;
    }

    // TODO: Can the zeroCrossingFilter have the same input and output?
    ZeroCrossingFilterPointer zeroCrossingFilter = ZeroCrossingFilterType::New();
    zeroCrossingFilter->SetInput(tempImage);
    zeroCrossingFilter->SetBackgroundValue(m_ValueOne);
    zeroCrossingFilter->SetForegroundValue(m_ValueZero);
    zeroCrossingFilter->Update();

    // The levelset image has a 0 where the zero contour exists and + outside
    // and - inside
    ImageRegionIterator<InputImageType> zIt(zeroCrossingFilter->GetOutput(), region);

    lIt.GoToBegin();
    zIt.GoToBegin();

    while (!lIt.IsAtEnd())
    {
      if (zIt.Get() == 0)
      {
        lIt.Set(0);
      }
      ++zIt;
      ++lIt;
    }
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
typename MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  TimeStepType
  MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
    CalculateChange()
{
  // Initialize to the maximum possible value
  TimeStepType     minTimeStep = NumericTraits<TimeStepType>::max();
  TimeStepType     timeStep;
  InputSpacingType spacing = this->m_LevelSet[0]->GetSpacing();

  // Calculate change across all the level-set functions
  for (IdCellType fId = 0; fId < this->m_FunctionCount; ++fId)
  {
    this->m_CurrentFunctionIndex = fId;

    const FiniteDifferenceFunctionPointer df = this->m_DifferenceFunctions[fId];

    SparseDataStruct * sparsePtr = this->m_SparseData[fId];

    FiniteDifferenceFunctionFloatOffsetType offset;
    ValueType                               gradientMagnitudeSqr, forward, backward, current;
    const ValueType                         MIN_NORM = 1.0e-6;

    void * globalData = df->GetGlobalDataPointer();

    NeighborhoodIterator<InputImageType> outputIt(
      df->GetRadius(), this->m_LevelSet[fId], this->m_LevelSet[fId]->GetRequestedRegion());

    if (m_BoundsCheckingActive == false)
    {
      outputIt.NeedToUseBoundaryConditionOff();
    }

    sparsePtr->m_UpdateBuffer.clear();
    sparsePtr->m_UpdateBuffer.reserve(sparsePtr->m_Layers[0]->Size());

    // Calculates the update values for the active layer indices in this
    // iteration.  Iterates through the active layer index list by evaluating
    // the update to the output image (level set image) at each
    // index.  Update values are stored in the update buffer.
    LayerConstIterator layerIt = sparsePtr->m_Layers[0]->Begin();

    unsigned int j;

    while (layerIt != sparsePtr->m_Layers[0]->End())
    {
      outputIt.SetLocation(layerIt->m_Value);

      current = outputIt.GetCenterPixel();

      // Calculate the offset to the surface from the current of this
      // neighborhood.  This is used by some level set functions in sampling a
      // speed, advection, or curvature term.
      if (this->GetInterpolateSurfaceLocation() && current != 0.0)
      {
        // Surface is at the zero crossing, so distance to surface is:
        // phi(x) / norm(grad(phi)), where phi(x) is the current of the
        // neighborhood.  The location is therefore
        // (i,j,k) - ( phi(x)/ norm(grad(phi))) * (grad(phi(x)) /
        // norm(grad(phi)) )
        gradientMagnitudeSqr = 0.0;

        for (j = 0; j < ImageDimension; ++j)
        {
          forward = outputIt.GetNext(j);
          backward = outputIt.GetPrevious(j);

          if (forward * backward >= 0)
          {
            //  Neighbors are same sign OR at least one neighbor is zero.
            // Pick the larger magnitude derivative.
            if (itk::Math::abs(forward - current) > itk::Math::abs(current - backward))
            {
              offset[j] = (forward - current) / spacing[j];
            }
            else
            {
              offset[j] = (current - backward) / spacing[j];
            }
          }
          else // Neighbors are opposite sign, pick the direction of 0 surface.
          {
            if (forward * current < 0)
            {
              offset[j] = (forward - current) / spacing[j];
            }
            else
            {
              offset[j] = (current - backward) / spacing[j];
            }
          }

          gradientMagnitudeSqr += offset[j] * offset[j];
        }

        // Adding sqrt imagedimension "extends the reach" of the
        // interpolation
        // to surfaces that pass close to the current of cells.  This is a
        // heuristic fudge factor that improves interpolation and reduces
        // "wiggling" at convergence.
        ValueType coeff = current * std::sqrt(ImageDimension + 0.5) / (gradientMagnitudeSqr + MIN_NORM);

        for (j = 0; j < ImageDimension; ++j)
        {
          offset[j] *= coeff;
        }

        sparsePtr->m_UpdateBuffer.push_back(df->ComputeUpdate(outputIt, globalData, offset));
      }
      else // Don't do interpolation
      {
        sparsePtr->m_UpdateBuffer.push_back(df->ComputeUpdate(outputIt, globalData));
      }

      ++layerIt;
    }

    // Ask the finite difference function to compute the time step for
    // this iteration.  We give it the global data pointer to use, then
    // ask it to free the global data memory.
    timeStep = df->ComputeGlobalTimeStep(globalData);
    df->ReleaseGlobalDataPointer(globalData);

    if (timeStep < minTimeStep)
    {
      minTimeStep = timeStep;
    }
  }

  minTimeStep = 0.2; // FIXME finally assigned to a constant

  return minTimeStep;
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::ApplyUpdate(
  TimeStepType dt)
{
  unsigned int j, k;

  for (IdCellType fId = 0; fId < this->m_FunctionCount; ++fId)
  {
    this->m_CurrentFunctionIndex = fId;

    SparseDataStruct * sparsePtr = this->m_SparseData[fId];
    unsigned int       t;

    StatusType up_to, up_search;
    StatusType down_to, down_search;

    LayerPointerType UpList[2];
    LayerPointerType DownList[2];

    for (j = 0; j < 2; ++j)
    {
      UpList[j] = LayerType::New();
      DownList[j] = LayerType::New();
    }

    // Process the active layer. This step will update the values in the
    // active layer as well as the values at indices that *will* become part
    // of the active layer when they are promoted/demoted. Also records
    // promotions, demotions in the UpList[0] and DownList[0] for current
    // active layer indices (i.e. those indices which will move inside or
    // outside
    // the active layers).
    this->UpdateActiveLayerValues(dt, UpList[0], DownList[0]);

    // Process the up/down lists. This is an iterative process which
    // proceeds outwards from the active layer. Each iteration generates the
    // list for the next iteration.

    // First process the status lists generated on the active layer.
    this->ProcessStatusList(UpList[0], UpList[1], 2, 1);
    this->ProcessStatusList(DownList[0], DownList[1], 1, 2);

    down_to = up_to = 0;
    up_search = 3;
    down_search = 4;
    j = 1;
    k = 0;
    while (down_search < static_cast<StatusType>(sparsePtr->m_Layers.size()))
    {
      this->ProcessStatusList(UpList[j], UpList[k], up_to, up_search);
      this->ProcessStatusList(DownList[j], DownList[k], down_to, down_search);

      if (up_to == 0)
      {
        up_to += 1;
      }
      else
      {
        up_to += 2;
      }

      down_to += 2;

      up_search += 2;
      down_search += 2;

      // Swap the lists so we can re-use the empty one.
      t = j;
      j = k;
      k = t;
    }

    // Process the outermost inside/outside layers in the sparse field.
    this->ProcessStatusList(UpList[j], UpList[k], up_to, m_StatusNull);
    this->ProcessStatusList(DownList[j], DownList[k], down_to, m_StatusNull);

    // Now we are left with the lists of indices which must be
    // brought into the outermost layers.  Bring UpList into last inside layer
    // and DownList into last outside layer.
    this->ProcessOutsideList(UpList[k], static_cast<signed char>(sparsePtr->m_Layers.size()) - 2);
    this->ProcessOutsideList(DownList[k], static_cast<signed char>(sparsePtr->m_Layers.size()) - 1);

    // Finally, we update all of the layer values (excluding the active layer,
    // which has already been updated).
    this->PropagateAllLayerValues();
  }

  this->m_CurrentFunctionIndex = 0;
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  ProcessOutsideList(LayerType * OutsideList, StatusType ChangeToStatus)
{
  SparseDataStruct * sparsePtr = this->m_SparseData[this->m_CurrentFunctionIndex];
  LayerNodeType *    node;

  // Push each index in the input list into its appropriate status layer
  // (ChangeToStatus) and update the status image value at that index.
  while (!OutsideList->Empty())
  {
    sparsePtr->m_StatusImage->SetPixel(OutsideList->Front()->m_Value, ChangeToStatus);
    node = OutsideList->Front();
    OutsideList->PopFront();
    sparsePtr->m_Layers[ChangeToStatus]->PushFront(node);
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  ProcessStatusList(LayerType * InputList,
                    LayerType * OutputList,
                    StatusType  ChangeToStatus,
                    StatusType  SearchForStatus)
{
  SparseDataStruct * sparsePtr = this->m_SparseData[this->m_CurrentFunctionIndex];

  unsigned int    i;
  bool            bounds_status;
  LayerNodeType * node;
  StatusType      neighbor_status;

  NeighborhoodIterator<StatusImageType> statusIt(m_NeighborList.GetRadius(),
                                                 sparsePtr->m_StatusImage,
                                                 this->m_LevelSet[this->m_CurrentFunctionIndex]->GetRequestedRegion());

  if (!m_BoundsCheckingActive)
  {
    statusIt.NeedToUseBoundaryConditionOff();
  }

  // Push each index in the input list into its appropriate status layer
  // (ChangeToStatus) and update the status image value at that index.
  // Also examine the neighbors of the index to determine which need to go
  // onto the output list (search for SearchForStatus).
  while (!InputList->Empty())
  {
    statusIt.SetLocation(InputList->Front()->m_Value);
    statusIt.SetCenterPixel(ChangeToStatus);

    node = InputList->Front(); // Must unlink from the input list
    InputList->PopFront();     // before transferring to another list.
    sparsePtr->m_Layers[ChangeToStatus]->PushFront(node);

    // Iterate through the neighbors of this status-changed node
    for (i = 0; i < m_NeighborList.GetSize(); ++i)
    {
      neighbor_status = statusIt.GetPixel(m_NeighborList.GetArrayIndex(i));

      // Have we bumped up against the boundary?  If so, turn on bounds
      // checking.
      if (neighbor_status == m_StatusBoundaryPixel)
      {
        m_BoundsCheckingActive = true;
      }

      // Find neighbors that move into the list prior to the ChangeToStatus
      if (neighbor_status == SearchForStatus)
      {
        // mark this pixel so we don't add it twice.
        statusIt.SetPixel(m_NeighborList.GetArrayIndex(i), m_StatusChanging, bounds_status);
        if (bounds_status == true)
        {
          node = sparsePtr->m_LayerNodeStore->Borrow();
          node->m_Value = statusIt.GetIndex() + m_NeighborList.GetNeighborhoodOffset(i);
          OutputList->PushFront(node);
        } // else this index was out of bounds.
      }
    }
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  UpdateActiveLayerValues(TimeStepType dt, LayerType * UpList, LayerType * DownList)
{
  SparseDataStruct * sparsePtr = this->m_SparseData[this->m_CurrentFunctionIndex];

  // This method scales the update buffer values by the time step and adds
  // them to the active layer pixels.  New values at an index which fall
  // outside of the active layer range trigger that index to be placed on the
  // "up" or "down" status list.  The neighbors of any such index are then
  // assigned new values if they are determined to be part of the active list
  // for the next iteration (i.e. their values will be raised or lowered into
  // the active range).

  // These need to take into account the spacing ??
  const ValueType LOWER_ACTIVE_THRESHOLD = -(m_ConstantGradientValue / 2.0);
  const ValueType UPPER_ACTIVE_THRESHOLD = m_ConstantGradientValue / 2.0;

  ValueType      new_value, temp_value;
  LayerNodeType *node, *release_node;
  StatusType     neighbor_status;
  unsigned int   i, idx;
  bool           bounds_status, flag;

  LayerIterator             layerIt;
  UpdateBufferConstIterator updateIt;

  NeighborhoodIterator<InputImageType> outputIt(m_NeighborList.GetRadius(),
                                                this->m_LevelSet[this->m_CurrentFunctionIndex],
                                                this->m_LevelSet[this->m_CurrentFunctionIndex]->GetRequestedRegion());

  NeighborhoodIterator<StatusImageType> statusIt(m_NeighborList.GetRadius(),
                                                 sparsePtr->m_StatusImage,
                                                 this->m_LevelSet[this->m_CurrentFunctionIndex]->GetRequestedRegion());

  // If bounds checking is turned on
  if (!m_BoundsCheckingActive)
  {
    outputIt.NeedToUseBoundaryConditionOff();
    statusIt.NeedToUseBoundaryConditionOff();
  }

  // Iterate over the update buffer and active layer
  // Both are the same size
  layerIt = sparsePtr->m_Layers[0]->Begin();
  updateIt = sparsePtr->m_UpdateBuffer.begin();

  while (layerIt != sparsePtr->m_Layers[0]->End())
  {
    outputIt.SetLocation(layerIt->m_Value);
    statusIt.SetLocation(layerIt->m_Value);

    new_value = this->CalculateUpdateValue(layerIt->m_Value, dt, outputIt.GetCenterPixel(), *updateIt);

    // If this index needs to be moved to another layer, then search its
    // neighborhood for indices that need to be pulled up/down into the
    // active layer. Set those new active layer values appropriately,
    // checking first to make sure they have not been set by a more
    // influential neighbor.

    //   ...But first make sure any neighbors in the active layer are not
    // moving to a layer in the opposite direction.  This step is necessary
    // to avoid the creation of holes in the active layer.  The fix is simply
    // to not change this value and leave the index in the active set.

    if (new_value >= UPPER_ACTIVE_THRESHOLD)
    {
      // This index will move UP into a positive (outside) layer. Contour is
      // shrinking
      // into the negative layers.

      // First check for neighbors that belong to the active layer and moving
      // in the opposite direction.
      flag = false;
      for (i = 0; i < m_NeighborList.GetSize(); ++i)
      {
        if (statusIt.GetPixel(m_NeighborList.GetArrayIndex(i)) == m_StatusActiveChangingDown)
        {
          flag = true;
          break;
        }
      }

      if (flag == true)
      {
        ++layerIt;
        ++updateIt;
        continue;
      }

      // Search the neighborhood for inside indices.
      for (i = 0; i < m_NeighborList.GetSize(); ++i)
      {
        temp_value = new_value - m_ConstantGradientValue * m_PixelDistance[i];
        idx = m_NeighborList.GetArrayIndex(i);
        neighbor_status = statusIt.GetPixel(idx);
        // 1 is first negative layer that will come into the active layer
        if (neighbor_status == 1)
        {
          // Keep the smallest possible value for the new active node.  This
          // places the new active layer node closest to the zero level-set.
          if (outputIt.GetPixel(idx) < LOWER_ACTIVE_THRESHOLD ||
              itk::Math::abs(temp_value) < itk::Math::abs(outputIt.GetPixel(idx)))
          {
            UpdatePixel(this->m_CurrentFunctionIndex, idx, outputIt, temp_value, bounds_status);
          }
        }
      }
      // Push current active layer pixel into the uplist
      node = sparsePtr->m_LayerNodeStore->Borrow();
      node->m_Value = layerIt->m_Value;
      UpList->PushFront(node);
      statusIt.SetCenterPixel(m_StatusActiveChangingUp);

      // Now remove this pixel from the active list.
      release_node = layerIt.GetPointer();
      sparsePtr->m_Layers[0]->Unlink(release_node);
      sparsePtr->m_LayerNodeStore->Return(release_node);
    }

    else if (new_value < LOWER_ACTIVE_THRESHOLD)
    {
      // This index will move DOWN into a negative (inside) layer. 2 in the
      // positive sparse field will come in

      // First check for active layer neighbors moving in the opposite
      // direction.
      flag = false;
      for (i = 0; i < m_NeighborList.GetSize(); ++i)
      {
        if (statusIt.GetPixel(m_NeighborList.GetArrayIndex(i)) == m_StatusActiveChangingUp)
        {
          flag = true;
          break;
        }
      }

      if (flag == true)
      {
        ++layerIt;
        ++updateIt;
        continue;
      }

      // Search the neighborhood for outside indices.
      for (i = 0; i < m_NeighborList.GetSize(); ++i)
      {
        temp_value = new_value + m_ConstantGradientValue * m_PixelDistance[i];
        idx = m_NeighborList.GetArrayIndex(i);
        neighbor_status = statusIt.GetPixel(idx);
        if (neighbor_status == 2)
        {
          // Keep the smallest magnitude value for this active set node.  This
          // places the node closest to the active layer.
          if (outputIt.GetPixel(idx) >= UPPER_ACTIVE_THRESHOLD ||
              itk::Math::abs(temp_value) < itk::Math::abs(outputIt.GetPixel(idx)))
          {
            UpdatePixel(this->m_CurrentFunctionIndex, idx, outputIt, temp_value, bounds_status);
          }
        }
      }
      // Push current active layer pixel into the downlist
      node = sparsePtr->m_LayerNodeStore->Borrow();
      node->m_Value = layerIt->m_Value;
      DownList->PushFront(node);
      statusIt.SetCenterPixel(m_StatusActiveChangingDown);

      // Now remove this index from the active list.
      release_node = layerIt.GetPointer();
      sparsePtr->m_Layers[0]->Unlink(release_node);
      sparsePtr->m_LayerNodeStore->Return(release_node);
    }
    else
    {
      UpdatePixel(this->m_CurrentFunctionIndex, outputIt.Size() / 2, outputIt, new_value, bounds_status);
    }

    // Move to the next active layer pixel
    ++layerIt;
    ++updateIt;
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  InitializeActiveLayerValues()
{
  // Initialize all active layer pixels to values computed as distance
  // to the 0 contour. Similar to the fast marching initial seeds.

  const ValueType  MIN_NORM = 1.0e-6;
  InputSpacingType spacing = this->m_LevelSet[0]->GetSpacing();

  double temp;

  for (IdCellType i = 0; i < this->m_FunctionCount; i++)
  {
    SparseDataStruct * sparsePtr = this->m_SparseData[i];

    InputImagePointer levelset = this->m_LevelSet[i];

    typename LayerType::ConstIterator         activeIt;
    ConstNeighborhoodIterator<InputImageType> outputIt(
      m_NeighborList.GetRadius(), levelset, levelset->GetRequestedRegion());

    sparsePtr->m_UpdateBuffer.clear();
    sparsePtr->m_UpdateBuffer.reserve(sparsePtr->m_Layers[0]->Size());

    unsigned int center; // index to active layer pixel
    center = outputIt.Size() / 2;
    ValueType dx, gradientMagnitude, gradientMagnitudeSqr, distance, forward, current, backward;

    // For all indices in the active layer...
    activeIt = sparsePtr->m_Layers[0]->Begin();
    while (activeIt != sparsePtr->m_Layers[0]->End())
    {
      // Interpolate on the (shifted) input image values at this index to
      // assign an active layer value in the output image.
      outputIt.SetLocation(activeIt->m_Value);

      gradientMagnitudeSqr = m_ValueZero;

      for (unsigned int j = 0; j < ImageDimension; ++j)
      {
        // Compute forward and backward pixel values
        forward = outputIt.GetPixel(center + m_NeighborList.GetStride(j));
        current = outputIt.GetCenterPixel();
        backward = outputIt.GetPixel(center - m_NeighborList.GetStride(j));

        if (forward * backward >= 0)
        {
          //  Neighbors are same sign OR at least one neighbor is zero.
          // Pick the larger magnitude derivative.
          if (::itk::Math::abs(forward - center) > ::itk::Math::abs(center - backward))
          {
            dx = (forward - current) / spacing[j];
          }
          else
          {
            dx = (current - backward) / spacing[j];
          }
        }
        else
        {
          // Choose the derivative closest to the 0 contour
          if (itk::Math::sgn(current * forward) == -1)
          {
            dx = (forward - current) / spacing[j];
          }
          else
          {
            dx = (current - backward) / spacing[j];
          }
        }
        gradientMagnitudeSqr += dx * dx;
      }
      gradientMagnitude = std::sqrt(gradientMagnitudeSqr) + MIN_NORM;

      // Compute the correct distance as phi(x)/gradientMagnitude
      distance = outputIt.GetCenterPixel() / gradientMagnitude;

      // Insert in the update buffer
      sparsePtr->m_UpdateBuffer.push_back(std::min(std::max(-MIN_NORM, distance), MIN_NORM));
      ++activeIt;
    }

    // Update the level-set image using the update buffer
    activeIt = sparsePtr->m_Layers[0]->Begin();
    while (activeIt != sparsePtr->m_Layers[0]->End())
    {
      // Update the accumulator value using the update buffer
      temp = static_cast<double>(sparsePtr->m_UpdateBuffer.front() - levelset->GetPixel(activeIt->m_Value));
      m_RMSSum += temp * temp;
      m_RMSCounter++;

      levelset->SetPixel(activeIt->m_Value, sparsePtr->m_UpdateBuffer.front());
      ++activeIt;
    }
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  PropagateAllLayerValues()
{
  for (IdCellType i = 0; i < this->m_FunctionCount; i++)
  {
    // Calls the UpdatePixel(...) function inside
    PropagateFunctionLayerValues(i);
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  PropagateFunctionLayerValues(unsigned int fId)
{
  SparseDataStruct * sparsePtr = this->m_SparseData[fId];

  // Update values in the first inside and first outside layers using the
  // active layer as a seed. Inside layers are odd numbers, outside layers are
  // even numbers.

  this->PropagateLayerValues(sparsePtr, 0, 1, 3, 1); // first inside
  this->PropagateLayerValues(sparsePtr, 0, 2, 4, 2); // first outside

  // Update the rest of the layers.
  for (unsigned int i = 1; i < sparsePtr->m_Layers.size() - 2; ++i)
  {
    this->PropagateLayerValues(sparsePtr, i, i + 2, i + 4, (i + 2) % 2);
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  PropagateLayerValues(SparseDataStruct * sparsePtr, StatusType from, StatusType to, StatusType promote, int InOrOut)
{
  // InOrOut indicates whether we are propagating in the negative/positive
  // region
  // of the level-set function

  unsigned int    i;
  ValueType       value_temp, delta;
  ValueType       value = NumericTraits<ValueType>::ZeroValue(); // warnings
  bool            found_neighbor_flag;
  LayerIterator   toIt;
  LayerNodeType * node;

  StatusType past_end = static_cast<StatusType>(sparsePtr->m_Layers.size()) - 1;

  // Are we propagating values inward (-1, more negative) or outward (1, more
  // positive)?
  delta = (InOrOut == 1) ? -1 : 1;

  NeighborhoodIterator<InputImageType>  outputIt(m_NeighborList.GetRadius(),
                                                this->m_LevelSet[sparsePtr->m_Index],
                                                this->m_LevelSet[sparsePtr->m_Index]->GetRequestedRegion());
  NeighborhoodIterator<StatusImageType> statusIt(
    m_NeighborList.GetRadius(), sparsePtr->m_StatusImage, this->m_LevelSet[sparsePtr->m_Index]->GetRequestedRegion());

  if (!m_BoundsCheckingActive)
  {
    outputIt.NeedToUseBoundaryConditionOff();
    statusIt.NeedToUseBoundaryConditionOff();
  }

  // Iterate over the to-layer to fill the values in the output image
  toIt = sparsePtr->m_Layers[to]->Begin();
  while (toIt != sparsePtr->m_Layers[to]->End())
  {
    // Set the iterator location in the status image
    OutputIndexType indexCurrent = toIt->m_Value;
    statusIt.SetLocation(indexCurrent);

    // Is this index marked for deletion? If the status image has
    // been marked with another layer's value, we need to delete this node
    // from the current list then skip to the next iteration.
    if (statusIt.GetCenterPixel() != to)
    {
      node = toIt.GetPointer();
      ++toIt;
      sparsePtr->m_Layers[to]->Unlink(node);
      sparsePtr->m_LayerNodeStore->Return(node);
      continue;
    }

    // Set the iterator location in the level-set image
    outputIt.SetLocation(toIt->m_Value);

    // We explore all neighbors to identify the closest from-layer node
    found_neighbor_flag = false;
    unsigned int indexNeighbor;
    for (i = 0; i < m_NeighborList.GetSize(); ++i)
    {
      // If this neighbor is in the "from" list, compare its absolute value
      // to any previous values found in the "from" list.  Keep the value
      // that will cause the to-layer to be closest to the zero level set.
      indexNeighbor = m_NeighborList.GetArrayIndex(i); // Get index
      if (statusIt.GetPixel(indexNeighbor) == from)    // if belongs to
                                                       // from-layer
      {
        // This value should be a distance in terms of spacing with neighbors
        // plus its current value

        InputPointType p1, p2;
        ValueType      dist = 0; // compute the distance between neighbors
        this->m_LevelSet[sparsePtr->m_Index]->TransformIndexToPhysicalPoint(statusIt.GetIndex(indexNeighbor), p1);
        this->m_LevelSet[sparsePtr->m_Index]->TransformIndexToPhysicalPoint(indexCurrent, p2);
        for (unsigned int j = 0; j < ImageDimension; j++)
        {
          dist += (p1[j] - p2[j]) * (p1[j] - p2[j]);
        }
        dist = delta * std::sqrt(dist);

        value_temp = dist + outputIt.GetPixel(indexNeighbor); // grab its
                                                              // value

        if (!found_neighbor_flag)
        {
          value = value_temp;
        }
        else
        {
          // Irrespective of negative/positive region, select the lowest
          // absolute minimum
          // value = delta * std::min( itk::Math::abs( value_temp ),
          // itk::Math::abs( value ) );
          if (InOrOut == 1) // inward
          {
            // Find the largest (least negative) neighbor
            if (value_temp > value)
            {
              value = value_temp;
            }
          }
          else
          {
            // Find the smallest (least positive) neighbor
            if (value_temp < value)
            {
              value = value_temp;
            }
          }
        }
        found_neighbor_flag = true;
      }
    }

    if (found_neighbor_flag)
    {
      // Set the new value using the smallest distance
      // found in our "from" neighbors.
      bool         bounds_status;
      unsigned int center = outputIt.Size() / 2;

      UpdatePixel(sparsePtr->m_Index, center, outputIt, value, bounds_status);

      // Update the rms change
      m_RMSSum += (value - outputIt.GetCenterPixel()) * (value - outputIt.GetCenterPixel());
      m_RMSCounter++;

      ++toIt;
    }
    else
    {
      // Did not find any neighbors on the "from" list, then promote this
      // node.  A "promote" value past the end of my sparse field size
      // means delete the node instead.  Change the status value in the
      // status image accordingly.
      node = toIt.GetPointer();
      ++toIt;
      sparsePtr->m_Layers[to]->Unlink(node);
      if (promote > past_end)
      {
        sparsePtr->m_LayerNodeStore->Return(node);
        // Reset the pixel status to null -- does not belong to sparse layer
        statusIt.SetCenterPixel(m_StatusNull);
        // Set the pixel to its default background value
        this->m_LevelSet[sparsePtr->m_Index]->SetPixel(indexCurrent, delta * this->m_BackgroundValue);
      }
      else
      {
        sparsePtr->m_Layers[promote]->PushFront(node);
        statusIt.SetCenterPixel(promote);
      }
    }
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  InitializeIteration()
{
  Superclass::InitializeIteration();

  m_RMSSum = 0.;
  m_RMSCounter = 0; // counter

  // Set the values in the output image for the active layer.
  this->InitializeActiveLayerValues();

  // Initialize layer values using the active layer as seeds
  this->PropagateAllLayerValues();

  // Determine the average RMS of change during this iteration
  if (m_RMSCounter == 0)
  {
    this->SetRMSChange(static_cast<double>(0.));
  }
  else
  {
    this->SetRMSChange(std::sqrt(m_RMSSum / m_RMSCounter));
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::Initialize()
{
  // Initialize m_PixelDistance values for the corresponding neighborhood list
  // This stores the distance between neighbors. Usually same as 1 except when
  // the image spacing is different.
  InputSpacingType spacing = this->m_LevelSet[0]->GetSpacing();
  OffsetType       offset;

  this->m_PixelDistance.clear();
  this->m_PixelDistance.resize(m_NeighborList.GetSize());
  for (unsigned int i = 0; i < m_NeighborList.GetSize(); ++i)
  {
    offset = m_NeighborList.GetNeighborhoodOffset(i);
    m_PixelDistance[i] = 0;
    for (unsigned int j = 0; j < ImageDimension; j++)
    {
      m_PixelDistance[i] += offset[j] * spacing[j] * offset[j] * spacing[j];
    }
    m_PixelDistance[i] = std::sqrt(m_PixelDistance[i]);
  }

  for (IdCellType fId = 0; fId < this->m_FunctionCount; ++fId)
  {
    SparseDataStruct * sparsePtr = this->m_SparseData[fId];

    // Allocate the status image.
    sparsePtr->m_StatusImage = StatusImageType::New();
    sparsePtr->m_StatusImage->SetRegions(this->m_LevelSet[fId]->GetRequestedRegion());
    sparsePtr->m_StatusImage->CopyInformation(this->m_LevelSet[fId]);
    sparsePtr->m_StatusImage->Allocate();
    sparsePtr->m_StatusImage->FillBuffer(m_StatusNull); // NonpositiveMin

    // Initialize the boundary pixels in the status image to
    // m_StatusBoundaryPixel values.  Uses the face calculator to find all of
    // the region faces.
    BFCType                        faceCalculator;
    typename BFCType::FaceListType faceList;

    // Set the difference function radius here
    typename BFCType::SizeType               sz = this->m_DifferenceFunctions[fId]->GetRadius();
    typename BFCType::FaceListType::iterator fit;

    // Compute the boundary pixel regions set in a container
    faceList = faceCalculator(sparsePtr->m_StatusImage, sparsePtr->m_StatusImage->GetRequestedRegion(), sz);

    // Iterate over the boundary region sets
    fit = faceList.begin();
    for (++fit; fit != faceList.end(); ++fit)
    {
      // For each region, set the pixel in m_StatusImage to
      // m_StatusBoundaryPixel
      ImageRegionIterator<StatusImageType> statusIt(sparsePtr->m_StatusImage, *fit);

      statusIt.GoToBegin();
      while (!statusIt.IsAtEnd())
      {
        statusIt.Set(m_StatusBoundaryPixel);
        ++statusIt;
      }
    }

    // Erase all existing layer lists -- element by element
    for (unsigned int i = 0; i < sparsePtr->m_Layers.size(); ++i)
    {
      while (!sparsePtr->m_Layers[i]->Empty())
      {
        sparsePtr->m_LayerNodeStore->Return(sparsePtr->m_Layers[i]->Front());
        sparsePtr->m_Layers[i]->PopFront();
      }
    }

    // Allocate the layers for the sparse field.
    sparsePtr->m_Layers.clear();
    sparsePtr->m_Layers.reserve(2 * this->m_NumberOfLayers + 1);

    while (sparsePtr->m_Layers.size() < (2 * this->m_NumberOfLayers + 1))
    {
      sparsePtr->m_Layers.push_back(LayerType::New());
    }

    // Throw an exception if we don't have enough layers.
    if (sparsePtr->m_Layers.size() < 3)
    {
      itkExceptionMacro(<< "Not enough layers have been allocated for the"
                           "sparse field.  Requires at least one layer.");
    }
  }

  // Set the background constants required to be set outside the sparse layer
  this->InitializeBackgroundConstants();

  // Construct the active layer and initialize the first layers inside and
  // outside of the active layer for all level-set functions.
  this->ConstructActiveLayer();

  for (IdCellType fId = 0; fId < this->m_FunctionCount; ++fId)
  {
    SparseDataStruct * sparsePtr = this->m_SparseData[fId];

    // Construct the rest of the non-active set layers using the first two
    // layers. Inside layers are odd numbers, outside layers are even numbers.
    // We need to loop from i = 1 to m_Layers.size()-2 since the last two layers
    // are constructed in the previous iteration
    for (unsigned int i = 1; i < sparsePtr->m_Layers.size() - 2; ++i)
    {
      // Construct layer i+2 from layer i. Note that layer i+1 is on the other
      // side
      this->ConstructLayer(sparsePtr, i, i + 2);
    }
  }

  // Set the values in the output image for the active layer.
  this->InitializeActiveLayerValues();

  // Initialize layer values using the active layer as seeds
  this->PropagateAllLayerValues();

  // Initialize pixels outside the sparse field layers to positive
  // and negative values, respectively. This is not necessary for the
  // calculations, but is useful for presenting a more intuitive output to the
  // filter.  See PostProcessOutput method for more information.
  this->InitializeBackgroundPixels();
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  InitializeBackgroundConstants()
{
  // Determine the maximum spacing to set the background pixel values
  // outside the sparse field
  float            maxSpacing = NumericTraits<float>::min();
  InputSpacingType spacing = this->m_LevelSet[0]->GetSpacing();

  for (unsigned int i = 0; i < ImageDimension; i++)
  {
    maxSpacing = std::max(maxSpacing, static_cast<float>(spacing[i]));
  }

  // Assign background pixels OUTSIDE the sparse field layers to a new level
  // set with value greater than the outermost layer.  Assign background pixels
  // INSIDE the sparse field layers to a new level set with value less than
  // the innermost layer.
  const auto max_layer = static_cast<ValueType>(this->m_NumberOfLayers);

  this->m_BackgroundValue = (max_layer + 1) * maxSpacing;
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  InitializeBackgroundPixels()
{
  for (IdCellType fId = 0; fId < this->m_FunctionCount; fId++)
  {
    SparseDataStruct * sparsePtr = this->m_SparseData[fId];

    ImageRegionConstIterator<StatusImageType> statusIt(sparsePtr->m_StatusImage,
                                                       this->m_LevelSet[fId]->GetRequestedRegion());

    ImageRegionIterator<InputImageType> outputIt(this->m_LevelSet[fId], this->m_LevelSet[fId]->GetRequestedRegion());

    outputIt.GoToBegin();
    statusIt.GoToBegin();

    while (!outputIt.IsAtEnd())
    {
      if (statusIt.Get() == m_StatusNull || statusIt.Get() == m_StatusBoundaryPixel)
      {
        if (outputIt.Get() > 0)
        {
          outputIt.Set(this->m_BackgroundValue);
        }
        if (outputIt.Get() < 0)
        {
          outputIt.Set(-this->m_BackgroundValue);
        }
      }
      ++outputIt;
      ++statusIt;
    }
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  ConstructActiveLayer()
{
  // We construct active layers for all level-set functions
  for (IdCellType fId = 0; fId < this->m_FunctionCount; fId++)
  {
    SparseDataStruct * sparsePtr = this->m_SparseData[fId];

    //  We find the active layer by searching for 0's in the zero crossing
    //  image (output image).  The first inside and outside layers are also
    //  constructed by searching the neighbors of the active layer in the
    //  (shifted) input image. Negative neighbors not in the active set are
    //  assigned to the inside, positive neighbors are assigned to the outside.
    //
    //  During construction we also check whether any of the layers of the
    //  active set (or the active set itself) is sitting on a boundary pixel
    //  location. If this is the case, then we need to do active bounds
    //  checking in the solver.

    NeighborhoodIterator<InputImageType> outputIt(
      m_NeighborList.GetRadius(), this->m_LevelSet[fId], this->m_LevelSet[fId]->GetRequestedRegion());

    NeighborhoodIterator<StatusImageType> statusIt(
      m_NeighborList.GetRadius(), sparsePtr->m_StatusImage, this->m_LevelSet[fId]->GetRequestedRegion());

    InputIndexType  center_index, offset_index;
    LayerNodeType * node;
    bool            bounds_status;
    StatusType      layer_number;

    // Determine image bounds for checking if sparse layers touch boundaries
    InputIndexType lowerBounds;
    InputSizeType  upperBounds;
    lowerBounds = this->m_LevelSet[fId]->GetRequestedRegion().GetIndex();
    upperBounds = this->m_LevelSet[fId]->GetRequestedRegion().GetSize();

    // Iterate over the output image
    outputIt.GoToBegin();
    while (!outputIt.IsAtEnd())
    {
      // Check if the center pixel has a value 0. The zeroCrossingFilter has
      // already placed 0s on the active layer pixels and 1 everywhere else.
      if (outputIt.GetCenterPixel() == m_ValueZero)
      {
        // Grab the neighborhood in the status image.
        center_index = outputIt.GetIndex();

        statusIt.SetLocation(center_index);

        // Check to see if any of the sparse field touches a boundary.  If so,
        // then activate bounds checking.
        for (unsigned int i = 0; i < ImageDimension; i++)
        {
          if ((center_index[i] + static_cast<InputOffsetValueType>(this->m_NumberOfLayers) >=
               (static_cast<InputIndexValueType>(upperBounds[i]) - 1)) ||
              center_index[i] - static_cast<InputOffsetValueType>(this->m_NumberOfLayers) <=
                static_cast<InputIndexValueType>(lowerBounds[i]))
          {
            m_BoundsCheckingActive = true;
          }
        }

        // Borrow a node from the store and set its value.
        node = sparsePtr->m_LayerNodeStore->Borrow();
        node->m_Value = center_index;

        // Add the node to the active list and set the status in the status
        // image.
        sparsePtr->m_Layers[0]->PushFront(node);
        statusIt.SetCenterPixel(0);

        // Search the neighborhood pixels for first inside & outside layer
        // members.  Construct these lists and set status list values.
        for (unsigned int i = 0; i < m_NeighborList.GetSize(); ++i)
        {
          // If the neighborhood pixel is not on the active layer
          // determine its sign to assign to outside or inside layers
          unsigned int neighborIndex = m_NeighborList.GetArrayIndex(i);
          if (outputIt.GetPixel(neighborIndex) != m_ValueZero)
          {
            // Determine if the neighbor belongs to layer 1 (inside) or 2
            // (outside)
            layer_number = (outputIt.GetPixel(neighborIndex) > 0) ? 2 : 1;

            // This check is to prevent the same pixel from being included more
            // than once
            // in the list
            if (statusIt.GetPixel(neighborIndex) == m_StatusNull)
            {
              statusIt.SetPixel(neighborIndex, layer_number, bounds_status);

              if (bounds_status) // In bounds.
              {
                offset_index = center_index + m_NeighborList.GetNeighborhoodOffset(i);
                node = sparsePtr->m_LayerNodeStore->Borrow();
                node->m_Value = offset_index;
                sparsePtr->m_Layers[layer_number]->PushFront(node);
              } // else do nothing.
            }
          }
        }
      }
      ++outputIt;
    }
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  ConstructLayer(SparseDataStruct * sparsePtr, StatusType from, StatusType to)
{
  LayerNodeType * node;
  bool            boundary_status;

  NeighborhoodIterator<StatusImageType> statusIt(
    m_NeighborList.GetRadius(), sparsePtr->m_StatusImage, this->m_LevelSet[sparsePtr->m_Index]->GetRequestedRegion());

  typename LayerType::ConstIterator fromIt;
  fromIt = sparsePtr->m_Layers[from]->Begin();

  // For all indices in the "from" layer...
  while (fromIt != sparsePtr->m_Layers[from]->End())
  {
    // Search the neighborhood of this index in the status image for
    // unassigned indices. Push those indices onto the "to" layer and
    // assign them values in the status image.  Status pixels outside the
    // boundary will be ignored.
    statusIt.SetLocation(fromIt->m_Value);
    for (unsigned int i = 0; i < m_NeighborList.GetSize(); ++i)
    {
      // If the pixel is not a boundary pixel or belongs to another layer
      unsigned int neighborIndex = m_NeighborList.GetArrayIndex(i);
      if (statusIt.GetPixel(neighborIndex) == m_StatusNull)
      {
        statusIt.SetPixel(neighborIndex, to, boundary_status);
        if (boundary_status == true) // in bounds
        {
          node = sparsePtr->m_LayerNodeStore->Borrow();
          node->m_Value = statusIt.GetIndex() + m_NeighborList.GetNeighborhoodOffset(i);
          sparsePtr->m_Layers[to]->PushFront(node);
        }
      }
    }
    ++fromIt;
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::
  PostProcessOutput()
{
  // Get the output pointer and clear its contents
  OutputImagePointer output = this->GetOutput();

  output->FillBuffer(NumericTraits<OutputPixelType>::ZeroValue());

  // Set the values in the levelset image for the active layer.
  this->InitializeActiveLayerValues();
  // Initialize layer values using the active layer as seeds.
  this->PropagateAllLayerValues();

  // Initialize pixels outside the sparse field layers to positive
  // and negative values, respectively. This is not necessary for the
  // calculations, but is useful for presenting a more intuitive output to the
  // filter.
  this->InitializeBackgroundPixels();

  for (IdCellType fId = 0; fId < this->m_FunctionCount; fId++)
  {
    InputImagePointer input = this->m_LevelSet[fId];
    InputPointType    origin = input->GetOrigin();

    // Local iterator
    ImageRegionIterator<InputImageType> inIt(this->m_LevelSet[fId], this->m_LevelSet[fId]->GetRequestedRegion());

    // In the context of the global coordinates
    OutputIndexType start;
    output->TransformPhysicalPointToIndex(origin, start);

    // Defining sub-region in the global coordinates
    OutputRegionType region;
    region.SetSize(input->GetRequestedRegion().GetSize());
    region.SetIndex(start);

    if (!input || !output)
    {
      itkExceptionMacro(<< "Either input and/or output is nullptr.");
    }

    ImageRegionIterator<OutputImageType> outIt(output, region);

    auto p = static_cast<OutputPixelType>(this->m_Lookup[fId]);

    inIt.GoToBegin();
    outIt.GoToBegin();

    while (!outIt.IsAtEnd())
    {
      if (inIt.Get() < 0)
      {
        outIt.Value() = p;
      }
      ++inIt;
      ++outIt;
    }
  }
}

template <typename TInputImage, typename TFeatureImage, typename TOutputImage, typename TFunction, typename TIdCell>
void
MultiphaseSparseFiniteDifferenceImageFilter<TInputImage, TFeatureImage, TOutputImage, TFunction, TIdCell>::PrintSelf(
  std::ostream & os,
  Indent         indent) const
{
  Superclass::PrintSelf(os, indent);

  os << indent << "m_IsoSurfaceValue: " << this->m_IsoSurfaceValue << std::endl;
  os << indent << "m_BoundsCheckingActive: " << m_BoundsCheckingActive;

  for (IdCellType i = 0; i < this->m_FunctionCount; i++)
  {
    SparseDataStruct * sparsePtr = this->m_SparseData[i];
    os << indent << "m_LayerNodeStore: " << std::endl;
    sparsePtr->m_LayerNodeStore->Print(os, indent.GetNextIndent());
    for (i = 0; i < sparsePtr->m_Layers.size(); i++)
    {
      os << indent << "m_Layers[" << i << "]: size=" << sparsePtr->m_Layers[i]->Size() << std::endl;
      os << indent << sparsePtr->m_Layers[i];
    }

    os << indent << "m_UpdateBuffer: size=" << static_cast<InputSizeValueType>(sparsePtr->m_UpdateBuffer.size())
       << " capacity = " << static_cast<InputSizeValueType>(sparsePtr->m_UpdateBuffer.capacity()) << std::endl;
  }

  os << indent << "Interpolate Surface Location " << m_InterpolateSurfaceLocation << std::endl;
  os << indent << "Number of Layers " << m_NumberOfLayers << std::endl;
  os << indent << "Value Zero " << static_cast<typename NumericTraits<ValueType>::PrintType>(m_ValueZero) << std::endl;
  os << indent << "Value One  " << static_cast<typename NumericTraits<ValueType>::PrintType>(m_ValueOne) << std::endl;
}
} // end namespace itk

#endif
