#ifndef __FPA__IMAGE__DIJKSTRAWITHSPHEREBACKTRACKING__HXX__
#define __FPA__IMAGE__DIJKSTRAWITHSPHEREBACKTRACKING__HXX__

#include <cmath>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIteratorWithIndex.h>

// -------------------------------------------------------------------------
template< class I, class C >
fpa::Image::DijkstraWithSphereBacktracking< I, C >::
DijkstraWithSphereBacktracking( )
  : Superclass( )
{
}

// -------------------------------------------------------------------------
template< class I, class C >
fpa::Image::DijkstraWithSphereBacktracking< I, C >::
~DijkstraWithSphereBacktracking( )
{
}

// -------------------------------------------------------------------------
template< class I, class C >
void fpa::Image::DijkstraWithSphereBacktracking< I, C >::
_BeforeMainLoop( )
{
  const I* img = this->GetInput( );
  typename I::SpacingType spac = img->GetSpacing( );
  double max_spac = spac[ 0 ];
  for( unsigned int d = 1; d < I::ImageDimension; ++d )
    max_spac =
      ( max_spac < double( spac[ d ] ) )? double( spac[ d ] ): max_spac;
  max_spac *= double( 3 );

  // Correct seeds
  for( unsigned int s = 0; s < this->m_Seeds.size( ); ++s )
  {
    _TNode seed = this->m_Seeds[ s ];
    _TRegion region = this->_Region( seed.Vertex, max_spac );
    itk::ImageRegionConstIteratorWithIndex< I > iIt( img, region );

    iIt.GoToBegin( );
    _TPixel max_value = iIt.Get( );
    for( ++iIt; !iIt.IsAtEnd( ); ++iIt )
    {
      if( iIt.Get( ) > max_value )
      {
        seed.Vertex = iIt.GetIndex( );
        seed.Parent = seed.Vertex;
        max_value = iIt.Get( );

      } // fi

    } // rof
    this->m_Seeds[ s ] = seed;

  } // rof

  // End initialization
  this->Superclass::_BeforeMainLoop( );
  this->m_Candidates.clear( );
}

// -------------------------------------------------------------------------
template< class I, class C >
void fpa::Image::DijkstraWithSphereBacktracking< I, C >::
_AfterMainLoop( )
{
  typedef itk::Image< bool, I::ImageDimension > _TMarkImage;

  // Finish base algorithm
  this->Superclass::_AfterMainLoop( );
  this->m_FinalTree.clear( );
  this->m_EndPoints.clear( );
  if( this->m_Candidates.size( ) == 0 )
    return;
  this->InvokeEvent( TEndEvent( ) );

  // Get input values
  const I* input = this->GetInput( );
  typename I::SpacingType spac = input->GetSpacing( );
  double max_spac = spac[ 0 ];
  for( unsigned int d = 1; d < I::ImageDimension; ++d )
    max_spac =
      ( max_spac < double( spac[ d ] ) )? double( spac[ d ] ): max_spac;
  max_spac *= double( 3 );

  // Prepare an object to hold marks
  typename _TMarkImage::Pointer marks = _TMarkImage::New( );
  marks->SetLargestPossibleRegion( input->GetLargestPossibleRegion( ) );
  marks->SetRequestedRegion( input->GetRequestedRegion( ) );
  marks->SetBufferedRegion( input->GetBufferedRegion( ) );
  marks->SetOrigin( input->GetOrigin( ) );
  marks->SetSpacing( input->GetSpacing( ) );
  marks->SetDirection( input->GetDirection( ) );
  marks->Allocate( );
  marks->FillBuffer( false );

  // Iterate over the candidates, starting from the candidates that
  // are near thin branches
  typename _TCandidates::const_reverse_iterator cIt =
    this->m_Candidates.rbegin( );
  for( int backId = 0; cIt != this->m_Candidates.rend( ); ++cIt )
  {
    // If pixel hasn't been visited, continue
    TVertex v = cIt->second;
    if( marks->GetPixel( v ) )
      continue;

    // Compute nearest start candidate
    _TRegion region = this->_Region( v, max_spac );
    itk::ImageRegionConstIteratorWithIndex< I > iIt( input, region );
    iIt.GoToBegin( );
    TVertex max_vertex = iIt.GetIndex( );
    _TPixel max_value = iIt.Get( );
    for( ++iIt; !iIt.IsAtEnd( ); ++iIt )
    {
      _TPixel value = iIt.Get( );
      if( value > max_value )
      {
        max_value = value;
        max_vertex = iIt.GetIndex( );

      } // fi

    } // rof

    // Keep endpoint
    if( marks->GetPixel( max_vertex ) )
      continue;
    backId++;
    this->m_EndPoints.push_back( max_vertex );

    // Construct path (at least the part that hasn't been iterated)
    do
    {
      if( this->m_FinalTree.find( max_vertex ) == this->m_FinalTree.end( ) )
      {
        // Mark a sphere around current point as visited
        double dist = std::sqrt( double( input->GetPixel( max_vertex ) ) );
        region = this->_Region( max_vertex, dist * double( 1.25 ) );
        itk::ImageRegionIteratorWithIndex< _TMarkImage >
          mIt( marks, region );
        for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
          mIt.Set( true );

        // Next vertex in current path
        this->InvokeEvent( TBacktrackingEvent( max_vertex, backId ) );
        this->m_FinalTree[ max_vertex ] = this->_Parent( max_vertex );

      } // fi
      max_vertex = this->_Parent( max_vertex );

    } while( max_vertex != this->_Parent( max_vertex ) );
    this->m_FinalTree[ max_vertex ] = max_vertex;
    this->InvokeEvent( TEndBacktrackingEvent( backId ) );

  } // rof
}

// -------------------------------------------------------------------------
template< class I, class C >
bool fpa::Image::DijkstraWithSphereBacktracking< I, C >::
_UpdateNeigh( _TNode& nn, const _TNode& n )
{
  C nc = this->_Cost( nn.Vertex, n.Vertex );
  if( TCost( 0 ) < nc )
  {
    nn.Cost = n.Cost + ( TCost( 1 ) / nc );
    nn.Result = nn.Cost;
    return( true );
  }
  else
    return( false );
}

// -------------------------------------------------------------------------
template< class I, class C >
bool fpa::Image::DijkstraWithSphereBacktracking< I, C >::
_UpdateResult( _TNode& n )
{
  bool ret = this->Superclass::_UpdateResult( n );
  if( ret )
  {
    TCost nc = this->_Cost( n.Vertex, n.Parent );
    if( TCost( 0 ) < nc )
    {
      TCost cc = n.Cost / nc;
      this->m_Candidates.insert( _TCandidate( cc, n.Vertex ) );
      /* TODO: debug code
         this->GetOutput( )->SetPixel( n.Vertex, cc );
      */
    } // fi

  } // fi
  return( ret );
}

// -------------------------------------------------------------------------
template< class I, class C >
typename fpa::Image::DijkstraWithSphereBacktracking< I, C >::
_TRegion fpa::Image::DijkstraWithSphereBacktracking< I, C >::
_Region( const TVertex& c, const double& r )
{
  typename I::ConstPointer input = this->GetInput( );
  typename I::SpacingType spac = input->GetSpacing( );
  _TRegion region = input->GetLargestPossibleRegion( );
  typename I::IndexType idx0 = region.GetIndex( );
  typename I::IndexType idx1 = idx0 + region.GetSize( );

  // Compute region size and index
  typename I::IndexType i0, i1;
  _TSize size;
  for( unsigned int d = 0; d < I::ImageDimension; ++d )
  {
    long s = long( std::ceil( r / double( spac[ d ] ) ) );
    i0[ d ] = c[ d ] - s;
    i1[ d ] = c[ d ] + s;

    if( i0[ d ] < idx0[ d ] ) i0[ d ] = idx0[ d ];
    if( i1[ d ] < idx0[ d ] ) i1[ d ] = idx0[ d ];
    if( i0[ d ] > idx1[ d ] ) i0[ d ] = idx1[ d ];
    if( i1[ d ] > idx1[ d ] ) i1[ d ] = idx1[ d ];
    size[ d ] = i1[ d ] - i0[ d ];

  }  // rof

  // Prepare region and return it
  region.SetIndex( i0 );
  region.SetSize( size );
  return( region );
}

#endif // __FPA__IMAGE__DIJKSTRAWITHSPHEREBACKTRACKING__HXX__

// eof - $RCSfile$