#ifndef __fpa__Image__SkeletonFilter__hxx__
#define __fpa__Image__SkeletonFilter__hxx__

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

// -------------------------------------------------------------------------
#define fpa_Image_SkeletonFilter_InputMacro( i_n, i_t, i_i )            \
  template< class _TDistanceMap, class _TCostMap >                      \
  typename fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::     \
  i_t* fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::         \
  Get##i_n( )                                                           \
  {                                                                     \
    return(                                                             \
      dynamic_cast< i_t* >( this->Superclass::GetInput( i_i ) )         \
      );                                                                \
  }                                                                     \
  template< class _TDistanceMap, class _TCostMap >                      \
  const                                                                 \
  typename fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::     \
  i_t* fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::         \
  Get##i_n( ) const                                                     \
  {                                                                     \
    return(                                                             \
      dynamic_cast< const i_t* >( this->Superclass::GetInput( i_i ) )   \
      );                                                                \
  }                                                                     \
  template< class _TDistanceMap, class _TCostMap >                      \
  void fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::         \
  Set##i_n( i_t* input )                                                \
  {                                                                     \
    this->Superclass::SetNthInput( i_i, input );                        \
  }

fpa_Image_SkeletonFilter_InputMacro( DistanceMap, TDistanceMap, 0 );
fpa_Image_SkeletonFilter_InputMacro( CostMap, TCostMap, 1 );
fpa_Image_SkeletonFilter_InputMacro( MinimumSpanningTree, TMST, 2 );

// -------------------------------------------------------------------------
#define fpa_Image_SkeletonFilter_OutputMacro( o_n, o_t, o_i )           \
  template< class _TDistanceMap, class _TCostMap >                      \
  typename fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::     \
  o_t* fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::         \
  Get##o_n( )                                                           \
  {                                                                     \
    return(                                                             \
      dynamic_cast< o_t* >( this->Superclass::GetOutput( o_i ) )        \
      );                                                                \
  }                                                                     \
  template< class _TDistanceMap, class _TCostMap >                      \
  const typename                                                        \
  fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::              \
  o_t* fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::         \
  Get##o_n( ) const                                                     \
  {                                                                     \
    return(                                                             \
      dynamic_cast< const o_t* >( this->Superclass::GetOutput( o_i ) )  \
      );                                                                \
  }

fpa_Image_SkeletonFilter_OutputMacro( Skeleton, TSkeleton, 0 );
fpa_Image_SkeletonFilter_OutputMacro( EndPoints, TIndices, 1 );
fpa_Image_SkeletonFilter_OutputMacro( Bifurcations, TIndices, 2 );
fpa_Image_SkeletonFilter_OutputMacro( Marks, TMarks, 3 );

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::
SkeletonFilter( )
  : Superclass( )
{
  this->SetNumberOfRequiredInputs( 3 );
  this->SetNumberOfRequiredOutputs( 3 );

  typename TIndices::Pointer end_points = TIndices::New( );
  typename TIndices::Pointer bifurcations = TIndices::New( );
  typename TSkeleton::Pointer skeleton = TSkeleton::New( );
  typename TMarks::Pointer marks = TMarks::New( );
  this->SetNthOutput( 0, skeleton.GetPointer( ) );
  this->SetNthOutput( 1, end_points.GetPointer( ) );
  this->SetNthOutput( 2, bifurcations.GetPointer( ) );
  this->SetNthOutput( 3, marks.GetPointer( ) );
}

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

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
void fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::
GenerateData( )
{
  // 0. I/O objects
  const TDistanceMap* dmap = this->GetDistanceMap( );
  const TCostMap* cmap = this->GetCostMap( );
  const TMST* mst = this->GetMinimumSpanningTree( );
  TIndices* ep = this->GetEndPoints( );
  TIndices* bi = this->GetBifurcations( );
  TSkeleton* sk = this->GetSkeleton( );

  // 1. Check input correspondance
  // TODO

  // 2. Detect end-points
  this->_EndPoints( dmap, cmap, mst, ep->Get( ) );
  std::cout << "endpoints" << std::endl;

  // 3. Build skeleton and keep track of bifurcations
  this->_Skeleton( dmap, cmap, mst, ep->Get( ), bi->Get( ), sk );
}

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
void fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::
_EndPoints(
  const TDistanceMap* dmap,
  const TCostMap* cmap,
  const TMST* mst,
  TIndicesData& end_points
  )
{
  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;
  typedef itk::ImageRegionIteratorWithIndex< TMarks > _TMarksIt;

  static const double _eps = std::sqrt( double( TMarks::ImageDimension + 1 ) );

  // Some values
  auto marks = this->GetMarks( );
  marks->SetLargestPossibleRegion( dmap->GetLargestPossibleRegion( ) );
  marks->SetRequestedRegion( dmap->GetRequestedRegion( ) );
  marks->SetBufferedRegion( dmap->GetBufferedRegion( ) );
  marks->SetSpacing( dmap->GetSpacing( ) );
  marks->SetOrigin( dmap->GetOrigin( ) );
  marks->SetDirection( dmap->GetDirection( ) );
  marks->Allocate( );
  marks->FillBuffer( 0 );

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

    } // fi

  } // rof

  // BFS from maximum queue
  auto region = dmap->GetRequestedRegion( );
  double init_v = queue.begin( )->first;
  while( queue.size( ) > 0 )
  {
    // Get node
    auto nIt = queue.begin( );
    auto n = *nIt;
    queue.erase( nIt );

    unsigned char m = marks->GetPixel( n.second );
    if( m != 0 )
      continue;

    std::cout << queue.size( ) << std::endl;

    // Mark it and update end-points
    m |= 0x01;
    marks->SetPixel( n.second, m );
    end_points.insert( n.second );

    // Mark path
    auto spac = marks->GetSpacing( );
    typename TMST::TPath::Pointer path;
    mst->GetPath( path, n.second );
    for( unsigned long i = 0; i < path->GetSize( ); ++i )
    {
      TIndex idx = path->GetVertex( i );
      typename _TCostMap::PointType cnt;
      cmap->TransformIndexToPhysicalPoint( idx, cnt );
      double r = double( dmap->GetPixel( idx ) ) * _eps;

      TIndex i0, i1;
      for( unsigned int d = 0; d < TMarks::ImageDimension; ++d )
      {
        long off = long( std::ceil( r / double( spac[ d ] ) ) );
        if( off == 0 )
          off = 1;
        i0[ d ] = idx[ d ] - off;
        i1[ d ] = idx[ d ] + off;

        if( i0[ d ] < region.GetIndex( )[ d ] )
          i0[ d ] = region.GetIndex( )[ d ];

        if( i1[ d ] >= region.GetIndex( )[ d ] + region.GetSize( )[ d ] )
          i1[ d ] = region.GetIndex( )[ d ] + region.GetSize( )[ d ] - 1;

      } // rof

      typename TMarks::SizeType size;
      for( unsigned int d = 0; d < TMarks::ImageDimension; ++d )
        size[ d ] = i1[ d ] - i0[ d ] + 1;

      typename TMarks::RegionType neighRegion;
      neighRegion.SetIndex( i0 );
      neighRegion.SetSize( size );

      _TMarksIt mIt( marks, neighRegion );
      for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
      {
        TIndex w = mIt.GetIndex( );
        typename _TCostMap::PointType p;
        marks->TransformIndexToPhysicalPoint( w, p );
        if( cnt.EuclideanDistanceTo( p ) <= r )
          mIt.Set( mIt.Get( ) | 0x02 );

      } // rof

    } // rof

  } // elihw
}

// -------------------------------------------------------------------------
template< class _TDistanceMap, class _TCostMap >
void fpa::Image::SkeletonFilter< _TDistanceMap, _TCostMap >::
_Skeleton(
  const TDistanceMap* dmap,
  const TCostMap* cmap,
  const TMST* mst,
  const TIndicesData& end_points,
  TIndicesData& bifurcations,
  TSkeleton* skeleton
  )
{
  typedef typename TMST::TPath _TPath;

  for( auto eIt = end_points.begin( ); eIt != end_points.end( ); ++eIt )
  {
    typename _TPath::Pointer path;
    mst->GetPath( path, *eIt );
    skeleton->AddBranch( path );

  } // rof
}

#endif // __fpa__Image__SkeletonFilter__hxx__

// eof - $RCSfile$