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

#ifndef __CPEXTENSIONS__ALGORITHMS__INERTIAMEDIALNESS__H__
#define __CPEXTENSIONS__ALGORITHMS__INERTIAMEDIALNESS__H__

#include <map>
#include <itkImageFunction.h>

#include <cpExtensions/Algorithms/InertiaTensorFunction.h>
#include <itkImageRegionConstIteratorWithIndex.h>

namespace cpExtensions
{
  namespace Algorithms
  {
    /**
     */
    template< class I, class S = float >
    class InertiaMedialness
      : public itk::ImageFunction< I, S, S >
    {
    public:
      // Standard itk types
      typedef InertiaMedialness               Self;
      typedef itk::ImageFunction< I, S, S >   Superclass;
      typedef itk::SmartPointer< Self >       Pointer;
      typedef itk::SmartPointer< const Self > ConstPointer;

      // Types from base itk::ImageFunction
      typedef typename Superclass::InputType           TInput;
      typedef typename Superclass::OutputType          TOutput;
      typedef typename Superclass::PointType           TPoint;
      typedef typename Superclass::ContinuousIndexType TContIndex;
      typedef typename Superclass::IndexType           TIndex;
      typedef typename TIndex::OffsetType              TOffset;

      // Sparse buffer
      typedef std::map< TIndex, TOutput, typename TIndex::LexicographicCompare > TBuffer;

      typedef InertiaTensorFunction< S, I::ImageDimension > TInertia;

    public:
      itkNewMacro( Self );
      itkTypeMacro( InertiaMedialness, itkImageFunction );

      itkBooleanMacro( BufferResults );
      itkGetConstMacro( BufferResults, bool );
      itkGetConstMacro( MaxRadius, double );

      itkSetMacro( BufferResults, bool );
      itkSetMacro( MaxRadius, double );

    public:
      virtual void ResetBuffer( )
        {
          this->m_Buffer.clear( );
        }

      virtual TOutput Evaluate( const TPoint& p ) const
        {
          TIndex i;
          this->GetInputImage( )->TransformPhysicalPointToIndex( p, i );
          return( this->EvaluateAtIndex( i ) );
        }

      virtual TOutput EvaluateAtIndex( const TIndex& i ) const
        {
          TOutput res = TOutput( 0 );
          bool computed = false;
          if( this->m_BufferResults )
          {
            typename TBuffer::const_iterator bIt = this->m_Buffer.find( i );
            computed = ( bIt != this->m_Buffer.end( ) );
            res = ( computed )? bIt->second: res;

          } // fi

          if( !computed )
            res = this->_Evaluate( i );

          if( this->m_BufferResults )
            this->m_Buffer[ i ] = res;
          return( res );
        }

      virtual TOutput EvaluateAtContinuousIndex( const TContIndex& i ) const
        {
          TPoint p;
          this->GetInputImage( )->TransformContinuousIndexToPhysicalPoint( i, p );
          return( this->Evaluate( p ) );
        }

    protected:
      InertiaMedialness( )
        : Superclass( ),
          m_BufferResults( false ),
          m_MaxRadius( double( 1 ) )
        {
          this->m_Buffer.clear( );
        }

      virtual ~InertiaMedialness( )
        {
          this->m_Buffer.clear( );
        }

      virtual TOutput _Evaluate( const TIndex& idx ) const
        {
          const I* image = this->GetInputImage( );

          typename I::PointType p_i;
          image->TransformIndexToPhysicalPoint( idx, p_i );

          typename I::PointType max_p, min_p;
          for( unsigned int d = 0; d < I::ImageDimension; ++d )
          {
            max_p[ d ] = p_i[ d ] + this->m_MaxRadius;
            min_p[ d ] = p_i[ d ] - this->m_MaxRadius;

          } // rof
          TIndex max_i, min_i;
          image->TransformPhysicalPointToIndex( max_p, max_i );
          image->TransformPhysicalPointToIndex( min_p, min_i );

          typename I::RegionType in_region = image->GetRequestedRegion( );
          TIndex in_index = in_region.GetIndex( );
          TIndex in_last = in_index + in_region.GetSize( );
          typename I::SizeType size;
          for( unsigned int d = 0; d < I::ImageDimension; ++d )
          {
            if( min_i[ d ] < in_index[ d ] ) min_i[ d ] = in_index[ d ];
            if( max_i[ d ] < in_index[ d ] ) max_i[ d ] = in_index[ d ];
            if( min_i[ d ] >= in_last[ d ] ) min_i[ d ] = in_last[ d ];
            if( max_i[ d ] >= in_last[ d ] ) max_i[ d ] = in_last[ d ];

            size[ d ] = max_i[ d ] - min_i[ d ];

          } // rof

          typename I::RegionType region;
          region.SetIndex( min_i );
          region.SetSize( size );

          std::vector< typename TInertia::Pointer > inertias;
          itk::ImageRegionConstIteratorWithIndex< I > it( image, region );
          for( it.GoToBegin( ); !it.IsAtEnd( ); ++it )
          {
            TOffset off = it.GetIndex( ) - idx;
            unsigned long l1dist = std::abs( off[ 0 ] );
            for( unsigned int d = 1; d < I::ImageDimension; ++d )
              l1dist = ( std::abs( off[ d ] ) > l1dist )? std::abs( off[ d ] ): l1dist;

            typename TInertia::TPoint i_pnt;
            image->TransformIndexToPhysicalPoint( it.GetIndex( ), i_pnt );

            for( unsigned long l = 0; l < l1dist; ++l )
            {
              if( inertias.size( ) <= l )
                inertias.push_back( TInertia::New( ) );
              inertias[ l ]->AddMass( i_pnt.GetVectorFromOrigin( ), S( it.Get( ) ) );

              /* TODO
                 typename TInertias::iterator inIt = inertias.find( l );
                 if( inIt == inertias.end( ) )
                 inIt = inertias.insert( std::pair< unsigned long, typename TInertia::Pointer >( l, TInertia::New( ) ) ).first;
              */

            } // rof

          } // rof

          if( inertias.size( ) > 0 )
          {
            S res = S( 0 );
            for( unsigned int l = 0; l < inertias.size( ); ++l )
            {
              typename TInertia::TVector pv, r;
              typename TInertia::TMatrix pm;
              inertias[ l ]->GetEigenAnalysis( pm, pv, r );
              S v = pv.GetNorm( );
              if( l == 0 || v > res )
                res = v;

            } // rof
            return( res );
          }
          else
            return( TOutput( 0 ) );
        }

    private:
      // Purposely not implemented.
      InertiaMedialness( const Self& );
      void operator=( const Self& );

    protected:
      mutable TBuffer m_Buffer;
      bool m_BufferResults;

      double m_MaxRadius;
    };

  } // ecapseman

} // ecapseman

#ifndef ITK_MANUAL_INSTANTIATION
// TODO: #include <cpExtensions/Algorithms/InertiaMedialness.hxx>
#endif // ITK_MANUAL_INSTANTIATION

#endif // __CPEXTENSIONS__ALGORITHMS__INERTIAMEDIALNESS__H__

// eof - $RCSfile$