#ifndef __FPA__IMAGE__DIJKSTRAWITHENDPOINTDETECTION__HXX__
#define __FPA__IMAGE__DIJKSTRAWITHENDPOINTDETECTION__HXX__

#include <vector>
#include <set>
#include <itkImageRegionConstIteratorWithIndex.h>

// -------------------------------------------------------------------------
template< class I, class O >
typename fpa::Image::DijkstraWithEndPointDetection< I, O >::
TLabelImage* fpa::Image::DijkstraWithEndPointDetection< I, O >::
GetLabelImage( )
{
  return(
    dynamic_cast< TLabelImage* >(
      this->itk::ProcessObject::GetOutput( this->m_LabelImageIndex )
      )
    );
}

// -------------------------------------------------------------------------
template< class I, class O >
const typename fpa::Image::DijkstraWithEndPointDetection< I, O >::
TLabelImage* fpa::Image::DijkstraWithEndPointDetection< I, O >::
GetLabelImage( ) const
{
  return(
    dynamic_cast< const TLabelImage* >(
      this->itk::ProcessObject::GetOutput( this->m_LabelImageIndex )
      )
    );
}

// -------------------------------------------------------------------------
template< class I, class O >
void fpa::Image::DijkstraWithEndPointDetection< I, O >::
GraftLabelImage( itk::DataObject* obj )
{
  TLabelImage* lbl =
    dynamic_cast< TLabelImage* >(
      this->itk::ProcessObject::GetOutput( this->m_LabelImageIndex )
      );
  if( lbl != NULL )
    this->GraftNthOutput( this->m_LabelImageIndex, lbl );
}

// -------------------------------------------------------------------------
template< class I, class O >
fpa::Image::DijkstraWithEndPointDetection< I, O >::
DijkstraWithEndPointDetection( )
  : Superclass( )
{
  this->m_LabelImageIndex = this->GetNumberOfRequiredOutputs( );
  this->SetNumberOfRequiredOutputs( this->m_LabelImageIndex + 1 );
  this->itk::ProcessObject::SetNthOutput(
    this->m_LabelImageIndex, TLabelImage::New( )
    );
}

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

// -------------------------------------------------------------------------
template< class I, class O >
void fpa::Image::DijkstraWithEndPointDetection< I, O >::
_BeforeGenerateData( )
{
  // Correct seeds
  /* TODO
     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::_BeforeGenerateData( );
  this->m_Candidates.clear( );
}

// -------------------------------------------------------------------------
template< class I, class O >
void fpa::Image::DijkstraWithEndPointDetection< I, O >::
_AfterGenerateData( )
{
  this->Superclass::_AfterGenerateData( );

  // Finish base algorithm
  /* TODO
     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( ) );
  this->InvokeEvent( TStartBacktrackingEvent( ) );

  // 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 );
  TLabelImage* label = this->GetLabelImage( );
  label->FillBuffer( 0 );

  // Prepare an object to hold marks
  std::set< TVertex, TVertexCompare > tree_marks;
  /* TODO
     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;
  std::map< TLabel, std::pair< TVertex, TVertex > > branches;
  for( ; cIt != this->m_Candidates.rend( ); ++cIt )
  {
    // If pixel has been already labelled, pass
    TVertex v = cIt->second;
    if( label->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

    // Re-check labelling
    if( label->GetPixel( max_vertex ) != 0 )
      continue;
    this->m_EndPoints.push_back( max_vertex );

    // Get the path all the way to seed
    std::vector< TVertex > path;
    this->GetMinimumSpanningTree( )->
      GetPath( path, max_vertex, this->GetSeed( 0 ) );

    // Mark branches
    bool start = true;
    bool change = false;
    TVertex last_start = max_vertex;
    typename std::vector< TVertex >::const_iterator pIt = path.begin( );
    for( ; pIt != path.end( ); ++pIt )
    {
      this->InvokeEvent(
        TBacktrackingEvent( *pIt, this->m_NumberOfBranches )
        );
      if( start )
      {
        if( tree_marks.find( *pIt ) == tree_marks.end( ) )
        {
          tree_marks.insert( *pIt );

          // Mark a sphere around current point as visited
          double dist = std::sqrt( double( input->GetPixel( *pIt ) ) );
          region = this->_Region( max_vertex, dist * double( 1.5 ) );
          itk::ImageRegionIteratorWithIndex< TLabelImage >
            lIt( label, region );
          for( lIt.GoToBegin( ); !lIt.IsAtEnd( ); ++lIt )
            lIt.Set( this->m_NumberOfBranches );

          // Next vertex in current path
          // TODO: this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
          /*
            this->m_FullTree[ max_vertex ] =
            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
          */
        }
        else
        {
          // A bifurcation point has been reached!
          branches[ this->m_NumberOfBranches ] = std::pair< TVertex, TVertex >( last_start, *pIt );
          last_start = *pIt;
          this->m_BifurcationPoints.push_back( *pIt );
          this->m_NumberOfBranches++;
          start = false;

        } // fi
      }
      else
      {
        if(
          std::find(
            this->m_BifurcationPoints.begin( ),
            this->m_BifurcationPoints.end( ),
            *pIt
            ) == 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< TLabelImage >
            lIt( label, region );
          for( lIt.GoToBegin( ); !lIt.IsAtEnd( ); ++lIt )
            lIt.Set( this->m_NumberOfBranches );

          // Next vertex in current path
          /* TODO
             this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
             this->m_FullTree[ max_vertex ] =
             TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
          */
          change = true;
        }
        else
        {
          // A bifurcation point has been reached!
          // TODO: this->m_BifurcationPoints.push_back( max_vertex );
          branches[ this->m_NumberOfBranches ] = std::pair< TVertex, TVertex >( last_start, *pIt );

          last_start = *pIt;
          this->m_NumberOfBranches++;
          /* TODO
             this->m_FullTree[ max_vertex ] =
             TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
          */

        } // fi

      } // fi

    } // rof
    if( start || change )
      this->m_NumberOfBranches++;
    this->InvokeEvent( TEndBacktrackingEvent( ) );



    /*
      this->InvokeEvent( TEndBacktrackingEvent( ) );
    */

  } // rof

  // Re-enumerate labels
  std::map< TLabel, unsigned long > histo;
  for(
    typename std::set< TVertex, TVertexCompare >::iterator treeIt = tree_marks.begin( );
    treeIt != tree_marks.end( );
    ++treeIt
    )
    histo[ label->GetPixel( *treeIt ) ]++;

  for(
    typename std::map< TLabel, unsigned long >::iterator hIt = histo.begin( );
    hIt != histo.end( );
    ++hIt
    )
    std::cout << hIt->first << " " << hIt->second << std::endl;

  /*
  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 O >
void fpa::Image::DijkstraWithEndPointDetection< I, O >::
_SetResult( const TVertex& v, const _TNode& n )
{
  this->Superclass::_SetResult( v, n );

  TResult vv = TResult( this->_VertexValue( v ) );
  if( TResult( 0 ) < vv )
    this->m_Candidates.insert( _TCandidate( n.Result / vv, v ) );
}

// -------------------------------------------------------------------------
template< class I, class O >
typename fpa::Image::DijkstraWithEndPointDetection< I, O >::
_TRegion fpa::Image::DijkstraWithEndPointDetection< I, O >::
_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__DIJKSTRAWITHENDPOINTDETECTION__HXX__

// eof - $RCSfile$