#ifndef __fpa__Image__SkeletonFilter__hxx__
#define __fpa__Image__SkeletonFilter__hxx__

#include <fpa/Base/Functors/Inverse.h>
#include <fpa/Image/Functors/SimpleNeighborhood.h>
#include <itkImageRegionIteratorWithIndex.h>

// -------------------------------------------------------------------------
#define fpa_Image_SkeletonFilter_OutputMacro( o_n, o_t )                \
  template< class _TImage >                                             \
  typename fpa::Image::SkeletonFilter< _TImage >::                      \
  o_t* fpa::Image::SkeletonFilter< _TImage >::                          \
  Get##o_n( )                                                           \
  {                                                                     \
    return(                                                             \
      dynamic_cast< o_t* >(                                             \
        this->itk::ProcessObject::GetOutput( this->m_##o_n##Idx )       \
        )                                                               \
      );                                                                \
  }

fpa_Image_SkeletonFilter_OutputMacro( Skeleton, TSkeleton );
fpa_Image_SkeletonFilter_OutputMacro( Marks, TMarks );

// -------------------------------------------------------------------------
template< class _TImage >
fpa::Image::SkeletonFilter< _TImage >::
SkeletonFilter( )
  : Superclass( )
{
  typedef typename _TImage::PixelType _TPixel;
  typedef typename _TImage::Superclass _TImageBase;
  typedef fpa::Image::Functors::SimpleNeighborhood< _TImageBase > _TNeighFunc;
  typedef fpa::Base::Functors::Inverse< _TPixel, _TPixel > _TInvFunc;

  unsigned int nOutputs = this->GetNumberOfRequiredOutputs( );
  this->SetNumberOfRequiredOutputs( nOutputs + 2 );
  this->m_SkeletonIdx = nOutputs;
  this->m_MarksIdx = nOutputs + 1;

  typename TSkeleton::Pointer skeleton = TSkeleton::New( );
  typename TMarks::Pointer marks = TMarks::New( );
  this->SetNthOutput( this->m_SkeletonIdx, skeleton.GetPointer( ) );
  this->SetNthOutput( this->m_MarksIdx, marks.GetPointer( ) );

  typename _TNeighFunc::Pointer nfunc = _TNeighFunc::New( );
  nfunc->SetOrder( 2 );
  this->SetNeighborhoodFunction( nfunc );

  typename _TInvFunc::Pointer ifunc = _TInvFunc::New( );
  this->SetCostConversionFunction( ifunc );
}

// -------------------------------------------------------------------------
template< class _TImage >
fpa::Image::SkeletonFilter< _TImage >::
~SkeletonFilter( )
{
}

// -------------------------------------------------------------------------
template< class _TImage >
void fpa::Image::SkeletonFilter< _TImage >::
_BeforeGenerateData( )
{
  this->Superclass::_BeforeGenerateData( );
  this->m_SkeletonQueue.clear( );
}

// -------------------------------------------------------------------------
template< class _TImage >
void fpa::Image::SkeletonFilter< _TImage >::
_UpdateResult( const _TQueueNode& n )
{
  typedef typename _TSkeletonQueue::value_type _TSkeletonQueueValue;

  this->Superclass::_UpdateResult( n );
  double d = double( this->GetInput( )->GetPixel( n.Vertex ) );
  if( d >= double( 0 ) )
  {
    // Update skeleton candidates
    d += double( 1e-5 );
    double v = double( n.Result ) / ( d * d );
    this->m_SkeletonQueue.insert( _TSkeletonQueueValue( v, n.Vertex ) );

  } // fi
}

// -------------------------------------------------------------------------
template< class _TImage >
void fpa::Image::SkeletonFilter< _TImage >::
_AfterGenerateData( )
{
  this->Superclass::_AfterGenerateData( );

  std::vector< TIndex > end_points;
  this->_EndPoints( end_points );
  this->_Skeleton( end_points );
}

// -------------------------------------------------------------------------
template< class _TImage >
void fpa::Image::SkeletonFilter< _TImage >::
_EndPoints( std::vector< TIndex >& end_points )
{
  typedef itk::ImageRegionIteratorWithIndex< TMarks > _TMarksIt;

  static const double _eps = std::sqrt( double( Self::Dimension + 1 ) );
  auto input = this->GetInput( );
  auto mst = this->GetMinimumSpanningTree( );
  auto marks = this->GetMarks( );

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

  // BFS from maximum queue
  auto region = input->GetRequestedRegion( );
  while( this->m_SkeletonQueue.size( ) > 0 )
  {
    // Get node
    auto nIt = this->m_SkeletonQueue.begin( );
    auto n = *nIt;
    this->m_SkeletonQueue.erase( nIt );

    // Mark it and update end-points
    unsigned char m = marks->GetPixel( n.second );
    if( m != 0 )
      continue;
    marks->SetPixel( n.second, 1 );
    end_points.push_back( n.second );

    // Mark path
    TIndex it = n.second;
    TIndex p = mst->GetParent( it );
    while( it != p )
    {
      // TODO: tags->SetPixel( it, tags->GetPixel( it ) + 1 );
      it = p;
      p = mst->GetParent( it );

    } // elihw
    // TODO: tags->SetPixel( it, tags->GetPixel( it ) + 1 );

    /* TODO: use mst directly rather than computing paths
       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 _TImage::PointType cnt;
       input->TransformIndexToPhysicalPoint( idx, cnt );
       double r = double( input->GetPixel( idx ) ) * _eps;

       TIndex i0, i1;
       for( unsigned int d = 0; d < Self::Dimension; ++d )
       {
       long off = long( std::ceil( r / double( spac[ d ] ) ) );
       if( off < 3 )
       off = 3;
       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 _TImage::SizeType size;
       for( unsigned int d = 0; d < Self::Dimension; ++d )
       size[ d ] = i1[ d ] - i0[ d ] + 1;

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

       _TMarksIt mIt( marks, neighRegion );
       for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
       {
       TIndex w = mIt.GetIndex( );
       typename _TImage::PointType p;
       marks->TransformIndexToPhysicalPoint( w, p );
       mIt.Set( 1 );

       } // rof

       } // rof
    */

  } // elihw
}

// -------------------------------------------------------------------------
template< class _TImage >
void fpa::Image::SkeletonFilter< _TImage >::
_Skeleton( const std::vector< TIndex >& end_points )
{
  typedef itk::Image< unsigned long, Self::Dimension > _TTagsImage;
  typedef typename TMST::TPath _TPath;

  auto mst = this->GetMinimumSpanningTree( );
  auto skeleton = this->GetSkeleton( );

  // Tag branches
  typename _TTagsImage::Pointer tags = _TTagsImage::New( );
  tags->SetLargestPossibleRegion( mst->GetLargestPossibleRegion( ) );
  tags->SetRequestedRegion( mst->GetRequestedRegion( ) );
  tags->SetBufferedRegion( mst->GetBufferedRegion( ) );
  tags->Allocate( );
  tags->FillBuffer( 0 );
  for( auto eIt = end_points.begin( ); eIt != end_points.end( ); ++eIt )
  {
    TIndex it = *eIt;
    TIndex p = mst->GetParent( it );
    while( it != p )
    {
      tags->SetPixel( it, tags->GetPixel( it ) + 1 );
      it = p;
      p = mst->GetParent( it );

    } // elihw
    tags->SetPixel( it, tags->GetPixel( it ) + 1 );

  } // rof

  // Build paths (branches)
  for( auto eIt = end_points.begin( ); eIt != end_points.end( ); ++eIt )
  {
    TIndex it = *eIt;
    TIndex p = mst->GetParent( it );
    TIndex sIdx = *eIt;
    typename _TPath::Pointer path = _TPath::New( );
    path->SetReferenceImage( mst );
    while( it != p )
    {
      if( tags->GetPixel( sIdx ) != tags->GetPixel( it ) )
      {
        // Ok, a new branch has been added
        path->AddVertex( it );
        skeleton->AddBranch( path );

        // Update a new starting index
        path = _TPath::New( );
        path->SetReferenceImage( mst );
        sIdx = it;
      }
      else
        path->AddVertex( it );
      it = p;
      p = mst->GetParent( it );

    } // elihw

    // Finally, add last branch
    path->AddVertex( it );
    skeleton->AddBranch( path );

  } // rof
}

#endif // __fpa__Image__SkeletonFilter__hxx__

// eof - $RCSfile$