#ifndef __fpa__Image__EndPointsFilter__hxx__
#define __fpa__Image__EndPointsFilter__hxx__

#include <functional>
#include <map>
#include <queue>
#include <itkImageRegionConstIteratorWithIndex.h>

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
const typename fpa::Image::EndPointsFilter< _TDistanceMap, _TCostMap >::
TIndices& fpa::Image::EndPointsFilter< _TDistanceMap, _TCostMap >::
GetBifurcations( ) const
{
  return( this->m_Bifurcations );
}

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
const typename fpa::Image::EndPointsFilter< _TDistanceMap, _TCostMap >::
TIndices& fpa::Image::EndPointsFilter< _TDistanceMap, _TCostMap >::
GetEndPoints( ) const
{
  return( this->m_EndPoints );
}

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
void fpa::Image::EndPointsFilter< _TDistanceMap, _TCostMap >::
Compute( )
{
  typedef itk::ImageRegionConstIteratorWithIndex< _TDistanceMap > _TDistMapIt;
  typedef itk::ImageRegionConstIteratorWithIndex< _TCostMap > _TCostMapIt;
  typedef std::multimap< double, TIndex, std::greater< double > > _TQueue;
  typedef typename _TQueue::value_type _TQueueValue;

  // Create queue
  _TQueue queue;
  _TDistMapIt dIt(
    this->m_DistanceMap, this->m_DistanceMap->GetRequestedRegion( )
    );
  _TCostMapIt cIt(
    this->m_CostMap, this->m_CostMap->GetRequestedRegion( )
    );
  dIt.GoToBegin( );
  cIt.GoToBegin( );
  for( ; !dIt.IsAtEnd( ) && !cIt.IsAtEnd( ); ++dIt, ++cIt )
  {
    double d = double( dIt.Get( ) );
    if( d > 0 )
    {
      double v = double( cIt.Get( ) ) * d;
      queue.insert( _TQueueValue( v, dIt.GetIndex( ) ) );

    } // fi

  } // rof

  TIndices marks;
  while( queue.size( ) > 0 )
  {
    auto nIt = queue.begin( );
    auto n = *nIt;
    queue.erase( nIt );

    if( marks.find( n.second ) == marks.end( ) )
    {
      std::cout << queue.size( ) << " " << n.first << std::endl;
      marks.insert( n.second );
      this->m_EndPoints.insert( n.second );
      auto path = this->m_MST->GetPath( n.second );
      std::cout << path.size( ) << std::endl;
      for( auto pIt = path.begin( ); pIt != path.end( ); ++pIt )
      {
        double d = double( this->m_DistanceMap->GetPixel( *pIt ) );
        d = std::sqrt( std::fabs( d ) );
        typename _TCostMap::PointType center;
        this->m_CostMap->TransformIndexToPhysicalPoint( *pIt, center );

        std::queue< TIndex > q;
        TIndices m;
        q.push( *pIt );
        while( q.size( ) > 0 )
        {
          TIndex idx = q.front( );
          q.pop( );
          if( m.find( idx ) != m.end( ) )
            continue;
          m.insert( idx );
          marks.insert( idx );
          for( unsigned int x = 0; x < _TCostMap::ImageDimension; ++x )
          {
            for( int y = -1; y <= 1; y += 2 )
            {
              TIndex idx2 = idx;
              idx2[ x ] += y;
              typename _TCostMap::PointType c;
              this->m_CostMap->TransformIndexToPhysicalPoint( idx2, c );
              if( this->m_CostMap->GetRequestedRegion( ).IsInside( idx2 ) )
              {
                if( center.EuclideanDistanceTo( c ) <= ( d * 1.5 ) )
                  q.push( idx2 );

              } // fi

            } // rof

          } // rof

        } // elihw

      } // rof

    } // fi

  } // elihw
}

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
fpa::Image::EndPointsFilter< _TDistanceMap, _TCostMap >::
EndPointsFilter( )
  : Superclass( )
{
}

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
fpa::Image::EndPointsFilter< _TDistanceMap, _TCostMap >::
~EndPointsFilter( )
{
}

#endif // __fpa__Image__EndPointsFilter__hxx__

// eof - $RCSfile$