/*=========================================================================

  Copyright (c) Kitware, Inc.
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/VolViewCopyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#ifndef __itkImageFunctionConditionalPathConstIterator_txx
#define __itkImageFunctionConditionalPathConstIterator_txx

#include "itkImageFunctionConditionalPathConstIterator.h"
#include "itkImageRegionConstIterator.h"

namespace itk
{

template<class TImage, class TFunction>
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::ImageFunctionConditionalPathConstIterator(const ImageType *imagePtr,
      FunctionType *fnPtr, PointContainerType & targetPoints, double searchRadius ) :
  m_FullyConnected(false)
{
  m_HasReachedTarget = false;
  this->m_Image = imagePtr;
  m_Function = fnPtr;
  m_StartIndices.clear();
  m_SearchRadius = searchRadius;
  m_SearchRadiusSquared = m_SearchRadius * m_SearchRadius;

  m_TargetPoints = targetPoints;
  const unsigned int n = m_TargetPoints.size();
  m_TargetIndices.resize(n);

  IndexType index;
  for (int i = 0 ; i < n; i++)
    {
    this->m_Image->TransformPhysicalPointToIndex( m_TargetPoints[i], index );
    m_TargetIndices[i] = index;
    std::cout << "Target index " << i << " = " << m_TargetIndices[i] << std::endl;
    }

  // Set up the temporary image
  this->InitializeIterator();
}  


template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::AllocateTempImage()
{
  // Build a temporary image of chars for use in the flood algorithm. We will
  // allocate a region that's Min( SearchBBox, ImageRegion )
  m_TempPtr = TTempImage::New();

  SizeType size;
  IndexType index;
  index.Fill(0);
  for (unsigned int i = 0; i < NDimensions; i++)
    {
    size[i] = static_cast< unsigned int >((2 * m_SearchRadius) / m_ImageSpacing[i] + 3.5);
    index[i] = -1 * static_cast< IndexValueType >(m_SearchRadius/ m_ImageSpacing[i] + 1 );
    }

  RegionType region( index, size );

  m_TempPtr->SetLargestPossibleRegion( region );
  m_TempPtr->SetBufferedRegion( region );
  m_TempPtr->SetRequestedRegion( region );
  m_TempPtr->Allocate();
}

template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::InitializeIterator()
{
  // Get the origin and spacing from the image in simple arrays
  m_ImageOrigin  = this->m_Image->GetOrigin();
  m_ImageSpacing = this->m_Image->GetSpacing();
  m_ImageRegion  = this->m_Image->GetBufferedRegion();
  
  // Build and setup the neighborhood iterator
  typename NeighborhoodIteratorType::RadiusType radius; radius.Fill(1);
  
  NeighborhoodIteratorType tmp_iter(radius, this->m_Image, m_ImageRegion);
  m_NeighborhoodIterator = tmp_iter;
  
  setConnectivity(&m_NeighborhoodIterator, m_FullyConnected);

  this->AllocateTempImage();
}


template<class TImage, class TFunction>
bool
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::IsNotFilled( const IndexType & index )
{
  for (unsigned int i = 0; i < NDimensions; i++)
    {
    m_TempIndex[i] = index[i] - m_SourceIndex[i];
    }

  return (m_TempPtr->GetPixel( m_TempIndex ) == 0);
}


template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::Fill( const IndexType & index )
{
  for (unsigned int i = 0; i < NDimensions; i++)
    {
    m_TempIndex[i] = index[i] - m_SourceIndex[i];
    }

  m_TempPtr->SetPixel( m_TempIndex, 2 );
}


template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::Remove( const IndexType & index )
{
  for (unsigned int i = 0; i < NDimensions; i++)
    {
    m_TempIndex[i] = index[i] - m_SourceIndex[i];
    }

  m_TempPtr->SetPixel( m_TempIndex, 1 );
}


template<class TImage, class TFunction>
bool
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::IsWithinSearchRadius( const IndexType & index )
{
  this->m_Image->TransformIndexToPhysicalPoint(index, m_TempPoint);
  m_TempVector = m_TempPoint - m_SourcePoint;
  return (m_TempVector.GetSquaredNorm() <= m_SearchRadiusSquared);
}

template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::SetSourcePoint( const PointType & point )
{
  m_StartIndices.clear();
  m_SourcePoint = point;
  this->m_Image->TransformPhysicalPointToIndex(point, m_SourceIndex);
  m_StartIndices.push_back(m_SourceIndex);
}

template<class TImage, class TFunction>
bool
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::IsAtTarget( const IndexType & index )
{
  for (IndexConstIteratorType it = m_TargetIndices.begin();
       it != m_TargetIndices.end(); 
       ++it )
    {
    if (*it == index)
      {

      // The inner loop is to avoid extra increments in the outer loop
      // which is performed thousands of times. This inner loop will
      // at most be performed once.. hence this code bloat.
      m_TargetId = 0;
      for (IndexConstIteratorType it2 = m_TargetIndices.begin();
           it2 != m_TargetIndices.end(); ++it2, ++m_TargetId )
        {
        //std::cout << m_TargetPoints[m_TargetId] << std::endl;
        if (*it2 == index)
          {
          this->m_IsAtEnd = true;
          this->m_HasReachedTarget = true;
          this->m_TargetIndex = index;
          this->m_TargetPoint = m_TargetPoints[m_TargetId];
          return true;
          }
        }

      }
    }

  return false;
}


template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::DoFloodStep()
{
  // The index in the front of the queue should always be
  // valid and be inside since this is what the iterator
  // uses in the Set/Get methods. This is ensured by the
  // GoToBegin() method.

  // Take the index in the front of the queue  
  const IndexType & topIndex = m_IndexStack.front();

  if (this->IsAtTarget(topIndex))
    {
    // Clear the queue
    while (!m_IndexStack.empty())
      {
      m_IndexStack.pop();
      }

    return;
    }
  
  
  // We are explicitly not calling set location since only offsets of
  // the neighborhood iterator are accessed.
  typename NeighborhoodIteratorType::ConstIterator neighborIt =
    m_NeighborhoodIterator.Begin();
  const typename NeighborhoodIteratorType::ConstIterator neighborEnd =
    m_NeighborhoodIterator.End();
  
  for (; neighborIt != neighborEnd; ++neighborIt)
    {
    const OffsetType& offset = neighborIt.GetNeighborhoodOffset();
    const IndexType tempIndex = topIndex + offset;

    // If this is a valid index and have not been tested,
    // then test it.
    if ( m_ImageRegion.IsInside( tempIndex )
         && this->IsWithinSearchRadius(tempIndex) )
      {
      if ( this->IsNotFilled(tempIndex) )
        {
        // if it is inside, push it into the queue  
        if ( this->IsPixelIncluded( tempIndex ) )
          {
          m_IndexStack.push( tempIndex );
          this->Fill( tempIndex ); 
          }
        else  // If the pixel is outside
          {
          // Mark the pixel as outside and remove it from the queue.
          this->Remove( tempIndex );
          }
        }
      }
    } // Finished traversing neighbors
    
  // Now that all the potential neighbors have been 
  // inserted we can get rid of the pixel in the front
  m_IndexStack.pop();
    
  if( m_IndexStack.empty() )
    {
    this->m_IsAtEnd = true;
    }

}

template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::SetFullyConnected(const bool _arg)
{
  if (this->m_FullyConnected != _arg)
    {
    this->m_FullyConnected = _arg;
    setConnectivity(&m_NeighborhoodIterator, m_FullyConnected);
    }
}

template<class TImage, class TFunction>
bool
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::GetFullyConnected() const
{
  return this->m_FullyConnected;
}

template<class TImage, class TFunction>
void
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::GoToBegin()
{
  // Clear the queue
  while (!m_IndexStack.empty())
    {
    m_IndexStack.pop();
    }

  this->m_IsAtEnd = true;
  // Initialize the temporary image
  m_TempPtr->FillBuffer(
    NumericTraits<ITK_TYPENAME TTempImage::PixelType>::Zero
    );
  
  for ( unsigned int i = 0; i < m_StartIndices.size(); i++ )
    {
    if( this->m_Image->GetBufferedRegion().IsInside ( m_StartIndices[i] ) &&
        this->IsPixelIncluded(m_StartIndices[i]) )
      {
      // Push the seed onto the queue
      m_IndexStack.push(m_StartIndices[i]);
      
      // Obviously, we're at the beginning
      this->m_IsAtEnd = false;
      
      // Mark the start index in the temp image as inside the
      // function, neighbor check incomplete
      this->Fill(m_StartIndices[i]);
      }
    }
}

template<class TImage, class TFunction>
bool
ImageFunctionConditionalPathConstIterator<TImage, TFunction>
::IsPixelIncluded(const IndexType & index) const
{
  if (this->m_Function->EvaluateAtIndex(index))
    std::cout << index << " " << this->m_Image->GetPixel(index) << std::endl;
  return this->m_Function->EvaluateAtIndex(index);
}

} // end namespace itk

#endif