#ifndef __FPA__BASE__TREEEXTRACTOR__HXX__
#define __FPA__BASE__TREEEXTRACTOR__HXX__

#include <limits>

// -------------------------------------------------------------------------
template< class A >
const unsigned long fpa::Base::TreeExtractor< A >::INF_VALUE =
  std::numeric_limits< unsigned long >::max( ) >> 1;

// -------------------------------------------------------------------------
template< class A >
unsigned int fpa::Base::TreeExtractor< A >::
AddLeaf( const TVertex& leaf )
{
  this->m_Leafs.push_back( leaf );
  this->Modified( );
  return( this->m_Leafs.size( ) - 1 );
}

// -------------------------------------------------------------------------
template< class A >
unsigned int fpa::Base::TreeExtractor< A >::
GetNumberOfLeafs( ) const
{
  return( this->m_Leafs.size( ) );
}

// -------------------------------------------------------------------------
template< class A >
const typename fpa::Base::TreeExtractor< A >::
TVertex& fpa::Base::TreeExtractor< A >::
GetLeaf( const unsigned int& id ) const
{
  static const TVertex zero;
  if( id < this->m_Leafs.size( ) )
    return( this->m_Leafs[ id ] );
  else
    return( zero );
}

// -------------------------------------------------------------------------
template< class A >
fpa::Base::TreeExtractor< A >::
TreeExtractor( )
  : Superclass( )
{
}

// -------------------------------------------------------------------------
template< class A >
fpa::Base::TreeExtractor< A >::
~TreeExtractor( )
{
}

// -------------------------------------------------------------------------
template< class A >
void fpa::Base::TreeExtractor< A >::
_BeforeMainLoop( )
{
  this->Superclass::_BeforeMainLoop( );
  this->m_Tree.clear( );
}

// -------------------------------------------------------------------------
template< class A >
void fpa::Base::TreeExtractor< A >::
_AfterMainLoop( )
{
  this->Superclass::_AfterMainLoop( );
  
  // Prepare fronts graph using Floyd-Warshall
  unsigned long nSeeds = this->GetNumberOfSeeds( );
  _TMatrix dist( nSeeds, _TRow( nSeeds, Self::INF_VALUE ) );
  this->m_FrontPaths = dist;

  for( unsigned long i = 0; i < nSeeds; ++i )
  {
    for( unsigned long j = 0; j < nSeeds; ++j )
    {
      if( this->m_CollisionSites[ i ][ j ].second )
      {
        dist[ i ][ j ] = 1;
        dist[ j ][ i ] = 1;
        this->m_FrontPaths[ i ][ j ] = j;
        this->m_FrontPaths[ j ][ i ] = i;

      } // fi

    } // rof
    dist[ i ][ i ] = 0;
    this->m_FrontPaths[ i ][ i ] = i;

  } // rof
  for( unsigned long k = 0; k < nSeeds; ++k )
  {
    for( unsigned long i = 0; i < nSeeds; ++i )
    {
      for( unsigned long j = 0; j < nSeeds; ++j )
      {
        if( ( dist[ i ][ k ] + dist[ k ][ j ] ) < dist[ i ][ j ] )
        {
          dist[ i ][ j ] = dist[ i ][ k ] + dist[ k ][ j ];
          this->m_FrontPaths[ i ][ j ] = this->m_FrontPaths[ i ][ k ];

        } // fi

      } // rof

    } // rof

  } // rof

  // Build tree
  this->m_Tree.clear( );
  typename TVertices::const_iterator vIt = this->m_Leafs.begin( );
  for( ; vIt != this->m_Leafs.end( ); ++vIt )
  {
    TVertices path = this->_Path( this->m_Root, *vIt );
    typename TVertices::const_iterator pIt = path.begin( );
    TVertex parent = *pIt;
    for( ; pIt != path.end( ); ++pIt )
    {
      this->m_Tree[ *pIt ] = parent;
      parent = *pIt;

    } // rof

  } // rof
}

// -------------------------------------------------------------------------
template< class A >
typename fpa::Base::TreeExtractor< A >::
TVertices fpa::Base::TreeExtractor< A >::
_Path( const TVertex& s, const TVertex& e )
{
  TVertices pS, pE;

  // Check if both vertices belong to the same front or if collisions
  // should be analyzed.
  _TFrontId fs = this->_FrontId( s );
  _TFrontId fe = this->_FrontId( e );
  if( fs == fe )
  {
    // Path from start vertex until seed
    TVertex vIt = s;
    TVertex parent = this->_Parent( vIt );
    while( parent != vIt )
    {
      pS.push_back( vIt );
      vIt = parent;
      parent = this->_Parent( vIt );

    } // elihw
    pS.push_back( vIt );

    // Path from end vertex until seed
    vIt = e;
    parent = this->_Parent( vIt );
    while( parent != vIt )
    {
      pE.push_back( vIt );
      vIt = parent;
      parent = this->_Parent( vIt );

    } // elihw
    pE.push_back( vIt );

    // Erase duplicate vertices
    if( pS.size( ) > 0 && pE.size( ) > 0 )
    {
      bool cont = ( pS.back( ) == pE.back( ) );
      bool enter = false;
      TVertex lastVertex = pS.back( );
      while( cont )
      {
        enter = true;

        lastVertex = pS.back( );
        pS.pop_back( );
        pE.pop_back( );

        cont = ( pS.size( ) > 0 && pE.size( ) > 0 );
        if( cont )
          cont = ( pS.back( ) == pE.back( ) );

      } // elihw
      if( enter )
        pS.push_back( lastVertex );

    } // fi

    // Contatenate both paths
    pS.insert( pS.end( ), pE.rbegin( ), pE.rend( ) );
  }
  else
  {
    // Use this->m_FrontPaths from Floyd-Warshall
    if( this->m_FrontPaths[ fs ][ fe ] != Self::INF_VALUE )
    {
      // Compute front path
      std::vector< unsigned long > fpath;
      fpath.push_back( fs );
      while( fs != fe )
      {
        fs = this->m_FrontPaths[ fs ][ fe ];
        fpath.push_back( fs );

      } // elihw

      // Continue only if both fronts are connected
      unsigned int N = fpath.size( );
      if( 0 < N )
      {
        // First path: from start vertex to first collision
        pS = this->_Path(
          s, this->m_CollisionSites[ fpath[ 0 ] ][ fpath[ 1 ] ].first
          );

        // Intermediary paths
        for( unsigned int i = 1; i < N - 1; ++i )
        {
          pE = this->_Path(
            this->m_CollisionSites[ fpath[ i ] ][ fpath[ i - 1 ] ].first,
            this->m_CollisionSites[ fpath[ i ] ][ fpath[ i + 1 ] ].first
            );
          pS.insert( pS.end( ), pE.begin( ), pE.end( ) );

        } // rof

        // Final path: from last collision to end point
        pE = this->_Path(
          this->m_CollisionSites[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first,
          e
          );
        pS.insert( pS.end( ), pE.begin( ), pE.end( ) );

      } // fi

    } // fi

  } // fi
  return( pS );
}

#endif // __FPA__BASE__TREEEXTRACTOR__HXX__

// eof - $RCSfile$