#ifndef __FPA__BASE__MINIMUMSPANNINGTREE__HXX__
#define __FPA__BASE__MINIMUMSPANNINGTREE__HXX__

#include <limits>

// -------------------------------------------------------------------------
template< class _TSuperclass, class _TVertex >
const unsigned long fpa::Base::
MinimumSpanningTree< _TSuperclass, _TVertex >::INF_VALUE =
  std::numeric_limits< unsigned long >::max( );
  
// -------------------------------------------------------------------------
template< class _TSuperclass, class _TVertex >
void fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
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
  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
  this->Modified( );
}

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

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

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

  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( ) );

      } // fi

    } // fi

  } // fi
  return( path );
}

// -------------------------------------------------------------------------
template< class _TSuperclass, class _TVertex >
typename fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
TPoints fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
GetEuclideanPath( const TVertex& a ) const
{
  TPoints points;
  return( points );
}

// -------------------------------------------------------------------------
template< class _TSuperclass, class _TVertex >
typename fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
TPoints fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
GetEuclideanPath( const TVertex& a, const TVertex& b ) const
{
  TPoints points;
  return( points );
}

// -------------------------------------------------------------------------
template< class _TSuperclass, class _TVertex >
bool fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
IsDefinedInEuclideanSpace( ) const
{
  return( false );
}

// -------------------------------------------------------------------------
template< class _TSuperclass, class _TVertex >
void fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
SetNode(
  const TVertex& v, const TVertex& p,
  const short& fid, const double& cost
  )
{
  typedef typename TNodeQueue::value_type _TNodeQueueValue;
  if( this->m_FillNodeQueue )
    this->m_NodeQueue.insert( _TNodeQueueValue( cost, v ) );
}

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

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

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

#endif // __FPA__BASE__MINIMUMSPANNINGTREE__HXX__

// eof - $RCSfile$