// -------------------------------------------------------------------------
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// -------------------------------------------------------------------------

#ifndef __CPEXTENSIONS__ALGORITHMS__GULSUNTEKMEDIALNESS__HXX__
#define __CPEXTENSIONS__ALGORITHMS__GULSUNTEKMEDIALNESS__HXX__

#include <cmath>
#include <vnl/vnl_math.h>


#include <queue>
#include <map>
#include <set>




// -------------------------------------------------------------------------
template< class G >
cpExtensions::Algorithms::GulsunTekMedialness< G >::
GulsunTekMedialness( )
  : Superclass( ),
    m_MinRadius( double( 0 ) ),
    m_MaxRadius( double( 1 ) ),
    m_ProfileSampling( 4 ),
    m_RadialSampling( 10 )
{
}

// -------------------------------------------------------------------------
template< class G >
cpExtensions::Algorithms::GulsunTekMedialness< G >::
~GulsunTekMedialness( )
{
}

// -------------------------------------------------------------------------
template< class G >
typename cpExtensions::Algorithms::GulsunTekMedialness< G >::
TOutput cpExtensions::Algorithms::GulsunTekMedialness< G >::
_Evaluate( const TIndex& i ) const
{
  const G* gradient = this->GetInputImage( );
  typename G::RegionType region = gradient->GetRequestedRegion( );
  typename G::PointType i_P;
  gradient->TransformIndexToPhysicalPoint( i, i_P );

  std::queue< TIndex > q;
  std::set< TIndex, typename TIndex::LexicographicCompare > m;
  std::map< TOffset, std::vector< TIndex >, typename TOffset::LexicographicCompare > profiles;
  q.push( i );

  while( !q.empty( ) )
  {
    // Get next node
    TIndex node = q.front( );
    q.pop( );
    if( m.find( node ) != m.end( ) )
      continue;
    m.insert( node );

    // Get neighborhood
    for( unsigned int d = 0; d < Self::Dimension; d++ )
    {
      for( int off = -1; off <= 1; off += 2 )
      {
        TIndex neighbor = node;
        neighbor[ d ] += off;
        if( region.IsInside( neighbor ) )
        {
          typename G::PointType neighbor_P;
          gradient->TransformIndexToPhysicalPoint( neighbor, neighbor_P );
          if( double( i_P.EuclideanDistanceTo( neighbor_P ) ) > this->m_MaxRadius )
            continue;
          
          // Normalize offset in terms of a l1 (manhattan) distance
          TOffset offset = neighbor - i;
          // std::cout << "Offset: " << offset << std::endl;

          /*
          int max_off = 0;
          for( unsigned int o = 0; o < Self::Dimension; ++o )
            max_off += std::abs( offset[ o ] );
          if( max_off == 0 )
            continue;
          bool normalized = true;
          TOffset normalized_offset;
          for( unsigned int o = 0; o < Self::Dimension && normalized; ++o )
          {
            if( offset[ o ] % max_off == 0 )
              normalized_offset[ o ] = offset[ o ] / max_off;
            else
              normalized = false;

          } // rof
          if( !normalized )
            normalized_offset = offset;
          std::cout << "offset: " << normalized_offset << std::endl;

          // Update profiles
          profiles[ normalized_offset ].push_back( neighbor );
          */

          // Update queue
          q.push( neighbor );

        } // fi

      } // rof

    } // rof

  } // elihw
  // std::cout << "HOLA: " << profiles.size( ) << std::endl;


  return( TOutput( 0 ) );
  /* TODO
     const G* in = this->GetInputImage( );
     double pi2n =
     double( 2 ) * double( vnl_math::pi ) /
     double( this->m_ProfileSampling );
     double rOff = this->m_MaxRadius / double( this->m_RadialSampling - 1 );
     double optR = double( 0 );
     TPoint pnt;
     in->TransformIndexToPhysicalPoint( i, pnt );

     // Main loop
     for( unsigned int cx = 0; cx < Self::Dimension - 1; cx++ )
     {
     for( unsigned int cy = cx + 1; cy < Self::Dimension; cy++ )
     {
     TProfile maxProfile( this->m_RadialSampling, double( 0 ) );
     for( unsigned int p = 0; p < this->m_ProfileSampling; p++ )
     {
     double a = pi2n * double( p );

     // Direction of this profile
     TVector dir;
     dir.Fill( TScalar( 0 ) );
     dir[ cx ] = TScalar( std::cos( a ) );
     dir[ cy ] = TScalar( std::sin( a ) );

     double maxrise = double( 0 );
     double maxfall = double( -1 );
     TProfile profile;
     for( unsigned int r = 0; r < this->m_RadialSampling; r++ )
     {
     double radius = double( r ) * rOff;
     TIndex idx;
     if(
     in->TransformPhysicalPointToIndex( pnt + ( dir * radius ), idx )
     )
     {
     TVector g = in->GetPixel( idx );
     double b = double( g.GetNorm( ) );
     if( double( g * dir ) < double( 0 ) )
     b *= double( -1 );
     maxrise = ( b > maxrise )? b: maxrise;
     if( radius >= this->m_MinRadius )
     maxfall = ( b < maxfall )? b: maxfall;
     profile.push_back( -b - maxrise );
     }
     else
     profile.push_back( double( 0 ) );

     } // rof

     for( unsigned int r = 0; r < this->m_RadialSampling; r++ )
     {
     double E = profile[ r ] / -maxfall;
     E = ( E < double( 0 ) )? double( 0 ): E;
     E = ( E > double( 1 ) )? double( 1 ): E;
     maxProfile[ r ] += E;

     } // rof

     } // rof

     for( unsigned int r = 0; r < this->m_RadialSampling; r++ )
     {
     double E = maxProfile[ r ] / double( this->m_RadialSampling );
     optR = ( E > optR )? E: optR;

     } // rof

     } // rof

     } // rof
     return( TScalar( optR ) );
  */
}

#endif // __CPEXTENSIONS__ALGORITHMS__GULSUNTEKMEDIALNESS__HXX__

// eof - $RCSfile$