// =========================================================================
// @author Leonardo Florez Valencia
// @email florez-l@javeriana.edu.co
// =========================================================================

#ifndef __fpa__Base__MinimumSpanningTree__hxx__
#define __fpa__Base__MinimumSpanningTree__hxx__

// -------------------------------------------------------------------------
template< class _TVertex, class _Superclass >
const typename fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
TCollisions& fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
GetCollisions( ) const
{
  return( this->m_Collisions );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _Superclass >
void fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
SetCollisions( const TCollisions& collisions )
{
  static const unsigned long _inf =
    std::numeric_limits< unsigned long >::max( );
  if( this->m_Collisions == collisions )
    return;

  this->m_Collisions = collisions;

  // Prepare a front graph
  unsigned long N = this->m_Collisions.size( );
  _TMatrix dist( N, _TRow( N, _inf ) );
  this->m_FrontPaths = dist;
  for( unsigned long i = 0; i < N; ++i )
  {
    for( unsigned long j = 0; j < N; ++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 < N; ++k )
  {
    for( unsigned long i = 0; i < N; ++i )
    {
      for( unsigned long j = 0; j < N; ++j )
      {
        // WARNING: you don't want a numeric overflow!!!
        unsigned long dik = dist[ i ][ k ];
        unsigned long dkj = dist[ k ][ j ];
        unsigned long sum = _inf;
        if( dik < _inf && dkj < _inf )
          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 _Superclass >
void fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
ClearSeeds( )
{
  this->m_Seeds.clear( );
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _Superclass >
void fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
AddSeed( const _TVertex& seed )
{
  this->m_Seeds.push_back( seed );
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _Superclass >
typename fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
TVertices fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
GetPath( const _TVertex& a ) const
{
  TVertices vertices;
  _TVertex it = a;
  _TVertex p = this->GetParent( it );
  while( it != p )
  {
    vertices.push_back( it );
    it = p;
    p = this->GetParent( it );

  } // elihw
  vertices.push_back( it );
  return( vertices );
}

// -------------------------------------------------------------------------
template< class _TVertex, class _Superclass >
typename fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
TVertices fpa::Base::MinimumSpanningTree< _TVertex, _Superclass >::
GetPath( const _TVertex& a, const _TVertex& b ) const
{
  static const unsigned long _inf =
    std::numeric_limits< unsigned long >::max( );

  TVertices vertices;
  TVertices pa = this->GetPath( a );
  TVertices pb = this->GetPath( b );
  if( pa.size( ) > 0 && pb.size( ) > 0 )
  {
    // Find front identifiers
    unsigned long ia = _inf, ib = _inf;
    unsigned long N = this->m_Seeds.size( );
    for( unsigned long i = 0; i < N; ++i )
    {
      if( this->m_Seeds[ i ] == pa[ pa.size( ) - 1 ] )
        ia = i;
      if( this->m_Seeds[ i ] == pb[ pb.size( ) - 1 ] )
        ib = i;

    } // rof

    // Check if there is a front-jump between given seeds
    if( ia != ib )
    {
      // Compute front path
      std::vector< long > fpath;
      fpath.push_back( ia );
      while( ia != ib )
      {
        ia = this->m_FrontPaths[ ia ][ ib ];
        fpath.push_back( ia );

      } // elihw

      // Continue only if both fronts are connected
      unsigned int N = fpath.size( );
      if( N > 0 )
      {
        // First path: from start vertex to first collision
        vertices = 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 - 1 ] ][ fpath[ i ] ].first,
              this->m_Collisions[ fpath[ i + 1 ] ][ fpath[ i ] ].first
              );
          for( long id = 0; id < ipath.size( ); ++id )
            vertices.push_back( ipath[ id ] );

        } // rof

        // Final path: from last collision to end point
        TVertices lpath =
          this->GetPath(
            this->m_Collisions[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first, b
            );
        for( long id = 0; id < lpath.size( ); ++id )
          vertices.push_back( lpath[ id ] );

      } // fi
    }
    else
    {
      // Ignore common part: find common ancestor
      long aIt = pa.size( ) - 1;
      long bIt = pb.size( ) - 1;
      bool cont = true;
      while( aIt >= 0 && bIt >= 0 && cont )
      {
        cont = ( pa[ aIt ] == pb[ bIt ] );
        aIt--;
        bIt--;

      } // elihw
      aIt++;
      bIt++;

      // Glue both parts
      for( long cIt = 0; cIt <= aIt; ++cIt )
        vertices.push_back( pa[ cIt ] );
      for( ; bIt >= 0; --bIt )
        vertices.push_back( pb[ bIt ] );

    } // fi

  } // fi
  return( vertices );
}

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

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

#endif // __fpa__Base__MinimumSpanningTree__hxx__

// eof - $RCSfile$