#ifndef __FPA__BASE__FASTMARCHING__HXX__
#define __FPA__BASE__FASTMARCHING__HXX__

#include <cmath>
#include <limits>

#include <itkExceptionObject.h>

// ---------------------------------------------------------------------
template< class V, class C, class VV, class VC, class B >
const double fpa::Base::FastMarching< V, C, VV, VC, B >::
LargestValue = std::numeric_limits< double >::max( );

// -------------------------------------------------------------------------
template< class V, class C, class VV, class VC, class B >
fpa::Base::FastMarching< V, C, VV, VC, B >::
FastMarching( )
  : Superclass( )
{
}

// -------------------------------------------------------------------------
template< class V, class C, class VV, class VC, class B >
fpa::Base::FastMarching< V, C, VV, VC, B >::
~FastMarching( )
{
}

// -------------------------------------------------------------------------
template< class V, class C, class VV, class VC, class B >
bool fpa::Base::FastMarching< V, C, VV, VC, B >::
_UpdateNeigh( _TNode& nn, const _TNode& n )
{
  bool r = true;
  if( !( this->_IsMarked( nn.Vertex ) ) )
  {
    double re;
    double F = double( this->_Value( nn.Vertex ) );
    r = this->_SolveEikonal( re, nn, F );
    if( r )
    {
      nn.Result = TResult( re );
      nn.Cost = TCost( re );

    } // fi

  } // fi
  return( r );
}

// -------------------------------------------------------------------------
template< class V, class C, class VV, class VC, class B >
bool fpa::Base::FastMarching< V, C, VV, VC, B >::
_SolveEikonal( double& re, const _TNode& n, const double& F )
{
  _TNodes neighs;
  this->_Neighs( n, neighs );

  // Compute interest nodes (i.e. the smallest valued in each dimension)
  _TNodes minNodes;
  unsigned int i = 0;
  while( i < neighs.size( ) )
  {
    V nm = neighs[ i++ ].Vertex;
    V np = neighs[ i++ ].Vertex;
    bool mm = this->_IsMarked( nm );
    bool mp = this->_IsMarked( np );
    TResult rm = this->_Result( nm );
    TResult rp = this->_Result( np );

    _TNode Ti;
    if( mm && mp )
      minNodes.push_back(
        ( rm < rp )?
        _TNode( nm, rm, n.FrontId ):
        _TNode( np, rp, n.FrontId )
        );
    else if( !mm && mp )
      minNodes.push_back( _TNode( np, rp, n.FrontId ) );
    else if( mm && !mp )
      minNodes.push_back( _TNode( nm, rm, n.FrontId ) );

  } // elihw
  std::sort( minNodes.begin( ), minNodes.end( ), _TNodeEikonalSort( ) );

  // Compute solution
  double al = double( 0 );
  double be = double( 0 );
  double ga = double( -1 ) / ( F * F );
  re = Self::LargestValue;

  typename _TNodes::const_iterator nIt = minNodes.begin( );
  bool valid = true;
  while( nIt != minNodes.end( ) && valid )
  {
    if( !( re < double( nIt->Result ) ) )
    {
      double d = this->_Norm( n.Vertex, nIt->Vertex );
      double s = double( 1 ) / ( d * d );
      double v = double( nIt->Result );

      al += s;
      be += v * s;
      ga += v * v * s;

      double de = ( be * be ) - ( al * ga );
      valid = !( de < double( 0 ) );
      if( valid )
        re = ( be + std::sqrt( de ) ) / al;
      nIt++;
    }
    else
      nIt = minNodes.end( );

  } // elihw
  return( valid );
}

#endif // __FPA__BASE__FASTMARCHING__HXX__

// eof - $RCSfile$