#ifndef __FPA__IMAGE__DIJKSTRAWITHSPHEREBACKTRACKING__HXX__
#define __FPA__IMAGE__DIJKSTRAWITHSPHEREBACKTRACKING__HXX__

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

// -------------------------------------------------------------------------
template< class I, class C >
typename fpa::Image::DijkstraWithSphereBacktracking< I, C >::
TMarkImage* fpa::Image::DijkstraWithSphereBacktracking< I, C >::
GetOutputMarkImage( )
{
  return(
    dynamic_cast< TMarkImage* >(
      this->itk::ProcessObject::GetOutput( 1 )
      )
    );
}

// -------------------------------------------------------------------------
template< class I, class C >
const typename fpa::Image::DijkstraWithSphereBacktracking< I, C >::
TMarkImage* fpa::Image::DijkstraWithSphereBacktracking< I, C >::
GetOutputMarkImage( ) const
{
  return(
    dynamic_cast< const TMarkImage* >(
      this->itk::ProcessObject::GetOutput( 1 )
      )
    );
}

// -------------------------------------------------------------------------
template< class I, class C >
fpa::Image::DijkstraWithSphereBacktracking< I, C >::
DijkstraWithSphereBacktracking( )
  : Superclass( ),
    m_NumberOfBranches( 0 )
{
  this->SetNumberOfRequiredOutputs( 2 );
  this->SetNthOutput( 0, I::New( ) );
  this->SetNthOutput( 1, TMarkImage::New( ) );
}

// -------------------------------------------------------------------------
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( 30 );

  // 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( )
{
  // Finish base algorithm
  this->Superclass::_AfterMainLoop( );
  this->m_FullTree.clear( );
  this->m_ReducedTree.clear( );
  this->m_EndPoints.clear( );
  this->m_BifurcationPoints.clear( );
  if( this->m_Candidates.size( ) == 0 )
    return;
  this->InvokeEvent( TEndEvent( ) );

  // Get some 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 = this->GetOutputMarkImage( );
  marks->FillBuffer( 0 );

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

    this->m_EndPoints.push_back( max_vertex );

    // Construct path (at least the part that hasn't been iterated)
    bool start = true;
    bool change = false;
    do
    {
      if( start )
      {
        if( this->m_FullTree.find( max_vertex ) == this->m_FullTree.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.5 ) );
          itk::ImageRegionIteratorWithIndex< TMarkImage >
            mIt( marks, region );
          for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
            mIt.Set( this->m_NumberOfBranches );

          // Next vertex in current path
          this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
          this->m_FullTree[ max_vertex ] =
            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
          // std::cout << "New: " << this->m_NumberOfBranches << std::endl;
        }
        else
        {
          // A bifurcation point has been reached!
          this->m_BifurcationPoints.push_back( max_vertex );
          this->m_NumberOfBranches++;

          this->m_FullTree[ max_vertex ] =
            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
          // std::cout << "Bifurcation: " << this->m_NumberOfBranches << std::endl;

          start = false;

        } // fi
      }
      else
      {
        if(
          std::find(
            this->m_BifurcationPoints.begin( ),
            this->m_BifurcationPoints.end( ),
            max_vertex
            ) == this->m_BifurcationPoints.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.5 ) );
          itk::ImageRegionIteratorWithIndex< TMarkImage >
            mIt( marks, region );
          for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
            mIt.Set( this->m_NumberOfBranches );

          // Next vertex in current path
          this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
          this->m_FullTree[ max_vertex ] =
            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
          change = true;
          // std::cout << "Change: " << this->m_NumberOfBranches << std::endl;
        }
        else
        {
          // A bifurcation point has been reached!
          // TODO: this->m_BifurcationPoints.push_back( max_vertex );
          this->m_NumberOfBranches++;
          this->m_FullTree[ max_vertex ] =
            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
          // std::cout << "Change bifurcation: " << this->m_NumberOfBranches << std::endl;

        } // fi

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

    } while( max_vertex != this->_Parent( max_vertex ) );
    if( start || change )
      this->m_NumberOfBranches++;
    this->m_FullTree[ max_vertex ] = TTreeNode( max_vertex, this->m_NumberOfBranches );

    this->InvokeEvent( TEndBacktrackingEvent( ) );

  } // rof

  std::map< TMark, unsigned long > histo;
  for(
    typename TTree::iterator treeIt = this->m_FullTree.begin( );
    treeIt != this->m_FullTree.end( );
    ++treeIt
    )
    histo[ treeIt->second.second ]++;

  std::map< TMark, TMark > changes;
  TMark last_change = 1;
  for( TMark i = 1; i <= this->m_NumberOfBranches; ++i )
  {
    if( histo[ i ] != 0 )
      changes[ i ] = last_change++;

  } // rof
  this->m_NumberOfBranches = changes.size( );

  for(
    typename TTree::iterator treeIt = this->m_FullTree.begin( );
    treeIt != this->m_FullTree.end( );
    ++treeIt
    )
  {
    TMark old = treeIt->second.second;
    treeIt->second.second = changes[ old ];

  } // fi

  // Construct reduced tree
  for(
    typename TVertices::const_iterator eIt = this->m_EndPoints.begin( );
    eIt != this->m_EndPoints.end( );
    ++eIt
    )
  {
    typename TTree::const_iterator tIt = this->m_FullTree.find( *eIt );
    if( tIt != this->m_FullTree.end( ) )
    {
      TMark id = tIt->second.second;
      do
      {
        tIt = this->m_FullTree.find( tIt->second.first );
        if( tIt == this->m_FullTree.end( ) )
          break;

      } while( tIt->second.second == id );
      this->m_ReducedTree[ *eIt ] = TTreeNode( tIt->first, id );

    } // fi

  } // rof

  for(
    typename TVertices::const_iterator bIt = this->m_BifurcationPoints.begin( );
    bIt != this->m_BifurcationPoints.end( );
    ++bIt
    )
  {
    typename TTree::const_iterator tIt = this->m_FullTree.find( *bIt );
    if( tIt != this->m_FullTree.end( ) )
    {
      TMark id = tIt->second.second;
      do
      {
        tIt = this->m_FullTree.find( tIt->second.first );
        if( tIt == this->m_FullTree.end( ) )
          break;

      } while( tIt->second.second == id );
      this->m_ReducedTree[ *bIt ] = TTreeNode( tIt->first, id );

    } // fi

  } // 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 )
  {
    typename I::PointType pnn, pn;
    this->GetInput( )->TransformIndexToPhysicalPoint( nn.Vertex, pnn );
    this->GetInput( )->TransformIndexToPhysicalPoint( n.Vertex, pn );


    nc += TCost( 1 );
    nn.Cost = n.Cost + ( TCost( pnn.EuclideanDistanceTo( pn ) ) / std::pow( nc, 4 ) );
    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$