#ifndef __FPA__BASE__MINIMUMSPANNINGTREE__HXX__
#define __FPA__BASE__MINIMUMSPANNINGTREE__HXX__

#include <limits>

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
const unsigned long fpa::Base::
MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::INF_VALUE =
  std::numeric_limits< unsigned long >::max( );

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
void fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
SetCollisions( const TCollisions& collisions )
{
  // Prepare a front graph
  this->m_Collisions = collisions;
  unsigned long nSeeds = this->m_Collisions.size( );
  _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_Collisions[ 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

  // Use Floyd-Warshall to compute all possible paths between fronts
  for( unsigned long k = 0; k < nSeeds; ++k )
  {
    for( unsigned long i = 0; i < nSeeds; ++i )
    {
      for( unsigned long j = 0; j < nSeeds; ++j )
      {
        // WARNING: you don't want a numeric overflow!!!
        unsigned long dik = dist[ i ][ k ];
        unsigned long dkj = dist[ k ][ j ];
        unsigned long sum = Self::INF_VALUE;
        if( dik < Self::INF_VALUE && dkj < Self::INF_VALUE )
          sum = dik + dkj;

        // Ok, continue Floyd-Warshall
        if( sum < dist[ i ][ j ] )
        {
          dist[ i ][ j ] = sum;
          this->m_FrontPaths[ i ][ j ] = this->m_FrontPaths[ i ][ k ];

        } // fi

      } // rof

    } // rof

  } // rof
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
typename
fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
TVertices
fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
GetPath( const _TVertex& a ) const
{
  TVertices path;
  if( this->_HasVertex( a ) )
    this->_Path( path, a );
  return( path );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
typename
fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
TVertices
fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
GetPath( const _TVertex& a, const _TVertex& b ) const
{
  TVertices path;

  // Check existence
  if( !this->_HasVertex( a ) || !this->_HasVertex( b ) )
    return( path );
  
  // Get front ids
  long fa = this->_FrontId( a ) - 1;
  long fb = this->_FrontId( b ) - 1;

  if( fa == fb )
  {
    // Get paths
    TVertices ap, bp;
    this->_Path( ap, a );
    this->_Path( bp, b );

    // Ignore common part: find common ancestor
    auto raIt = ap.rbegin( );
    auto rbIt = bp.rbegin( );
    while( *raIt == *rbIt && raIt != ap.rend( ) && rbIt != bp.rend( ) )
    {
      ++raIt;
      ++rbIt;

    } // elihw
    if( raIt != ap.rbegin( ) ) --raIt;
    if( rbIt != bp.rbegin( ) ) --rbIt;

    // Add part from a
    for( auto iaIt = ap.begin( ); iaIt != ap.end( ) && *iaIt != *raIt; ++iaIt )
      path.push_back( *iaIt );

    // Add part from b
    for( ; rbIt != bp.rend( ); ++rbIt )
      path.push_back( *rbIt );
  }
  else
  {
    // Use this->m_FrontPaths from Floyd-Warshall
    if( this->m_FrontPaths[ fa ][ fb ] < Self::INF_VALUE )
    {
      // Compute front path
      std::vector< long > fpath;
      fpath.push_back( fa );
      while( fa != fb )
      {
        fa = this->m_FrontPaths[ fa ][ fb ];
        fpath.push_back( fa );

      } // elihw

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

        // Intermediary paths
        for( unsigned int i = 1; i < N - 1; ++i )
        {
          TVertices ipath =
            this->GetPath(
              this->m_Collisions[ fpath[ i ] ][ fpath[ i - 1 ] ].first,
              this->m_Collisions[ fpath[ i ] ][ fpath[ i + 1 ] ].first
              );
          path.insert( path.end( ), ipath.begin( ), ipath.end( ) );

        } // rof

        // Final path: from last collision to end point
        TVertices lpath =
          this->GetPath(
            this->m_Collisions[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first, b
            );
        path.insert( path.end( ), lpath.begin( ), lpath.end( ) );

        // Remove repeated vertices
        auto p = path.begin( );
        auto q = p;
        q++;
        while( q != path.end( ) )
        {
          if( *q == *p )
          {
            p = path.erase( p );
            q = p;
          }
          else
            ++p;
          ++q;
          
        } // elihw

      } // fi

    } // fi

  } // fi
  return( path );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
void fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
SetNode(
  const _TVertex& vertex, const _TVertex& parent,
  const long& fid, const _TScalar& result
  )
{
  this->Get( )[ vertex ] = TParent( parent, TData( result, fid ) );
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
void fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
Clear( )
{
  this->Get( ).clear( );
  this->m_NodeQueue.clear( );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
MinimumSpanningTree( )
  : Superclass( ),
    m_FillNodeQueue( false )
{
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
~MinimumSpanningTree( )
{
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
bool fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
_HasVertex( const _TVertex& a ) const
{
  return( this->Get( ).find( a ) != this->Get( ).end( ) );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
long fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
_FrontId( const _TVertex& a ) const
{
  static const long MAX_ID = std::numeric_limits< long >::max( );
  auto i = this->Get( ).find( a );
  if( i != this->Get( ).end( ) )
    return( i->second.second.second );
  else
    return( MAX_ID );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _TScalar, class _TVertexCompare >
void fpa::Base::MinimumSpanningTree< _TVertex, _TScalar, _TVertexCompare >::
_Path( TVertices& path, const _TVertex& a ) const
{
  auto& m = this->Get( );
  auto it = m.find( a );
  if( it != m.end( ) )
  {
    do
    {
      path.push_back( it->first );
      it = m.find( it->second.first );

    } while( it->first != it->second.first );
    path.push_back( it->first );

  } // fi
}

#endif // __FPA__BASE__MINIMUMSPANNINGTREE__HXX__

// eof - $RCSfile$