#ifndef __CPM__ALGORITHMS__BASE__FINDCLOSESTPOINTINMESH__HXX__
#define __CPM__ALGORITHMS__BASE__FINDCLOSESTPOINTINMESH__HXX__

#include <limits>

// -------------------------------------------------------------------------
template< class M, class S >
void cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
SetMesh( const M* m )
{
  this->m_Mesh = m;
  this->Modified( );
}

// -------------------------------------------------------------------------
template< class M, class S >
void cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
Build( )
{
  // Fill in samples
  this->m_KDSample = TKDSample::New( );
  this->m_KDSample->SetMeasurementVectorSize( TKDVector::Dimension );

  TKDVector mv;
  for(
    unsigned long pId = 0;
    pId < this->m_Mesh->GetNumberOfPoints( );
    ++pId
    )
  {
    typename M::PointType pnt = this->m_Mesh->GetPoint( pId );
    for( unsigned int d = 0; d < TKDVector::Dimension; ++d )
      mv[ d ] = S( pnt[ d ] );
    mv.PointId = ( typename M::PointIdentifier )( pId );
    this->m_KDSample->PushBack( mv );

  } // rof

  // Compute kd-Tree
  this->m_KDGenerator = TKDGenerator::New( );
  this->m_KDGenerator->SetSample( this->m_KDSample );
  this->m_KDGenerator->SetBucketSize( this->m_BucketSize );
  this->m_KDGenerator->Update( );
  this->m_KDTree = this->m_KDGenerator->GetOutput( );
}

// -------------------------------------------------------------------------
template< class M, class S >
template< class P >
typename M::PointIdentifier cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
FindClosestPoint( const P& p ) const
{
  TKDVector query;
  for( unsigned int d = 0; d < TKDVector::Dimension; ++d )
    query[ d ] = S( p[ d ] );

  typename TKDTree::InstanceIdentifierVectorType neighs;
  std::vector< double > distances;
  this->m_KDTree->Search( query, this->m_BucketSize, neighs, distances );
  unsigned int minId = 0;
  double minD = distances[ 0 ];
  for( unsigned int i = 1; i < distances.size( ); ++i )
  {
    if( distances[ i ] < minD )
    {
      minId = i;
      minD = distances[ i ];

    } // fi

  } // rof
  std::cout << "TODO: " << minD << std::endl;
  return( neighs[ minId ] );
}

// -------------------------------------------------------------------------
template< class M, class S >
template< class P, class C >
typename M::PointIdentifier cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
FindClosestPoint( const P& p, const C& c ) const
{
  typename TKDVector::Superclass p1, p2, d;
  for( unsigned int i = 0; i < TKDVector::Dimension; ++i )
  {
    p1[ i ] = S( p[ i ] );
    p2[ i ] = S( c[ i ] );
    d[ i ] = p1[ i ] - p2[ i ];

  } // rof
  TKDScalar nd = d.GetNorm( );
  typename TKDVector::Superclass n = itk::CrossProduct( p2, p1 );
  this->_Dummy( this->m_KDTree->GetRoot( ), d, n, nd );

  return( 0 );
}

// -------------------------------------------------------------------------
template< class M, class S >
template< class V, class N >
void cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
_Dummy( TKDNode* root, const V& d, const V& n, const N& nd ) const
{
  typedef
    itk::Statistics::KdTreeWeightedCentroidNonterminalNode< TKDSample >
    _TNode;
  _TNode* node = dynamic_cast< _TNode* >( root );
  if( node != NULL )
  {
    // Get centroids
    S left = std::numeric_limits< S >::max( );
    S right = left;
    if( root->Left( ) != NULL )
    {
      if( !( root->Left( )->IsTerminal( ) ) )
      {
        typename _TNode::CentroidType c;
        root->Left( )->GetCentroid( c );
        left = Self::_LinePointDistance( c, d, n, nd );

      } // fi

    } // fi
    if( root->Right( ) != NULL )
    {
      if( !( root->Right( )->IsTerminal( ) ) )
      {
        typename _TNode::CentroidType c;
        root->Right( )->GetCentroid( c );
        right = Self::_LinePointDistance( c, d, n, nd );

      } // fi

    } // fi

    // Recursive!!!
    if( left <= right && root->Left( ) != NULL )
      this->_Dummy( root->Left( ), d, n, nd );
    else if( left > right && root->Right( ) != NULL )
      this->_Dummy( root->Right( ), d, n, nd );
    else
    {
      std::cout << "Something nasty happened" << std::endl;

    } // fi

  }
  else
  {
    std::cout << "Leaf!!!" << std::endl;

  } // fi

  /*
    _LinePointDistance( const P& p0, const D& d, const N& n, const T& nd )
    std::cout << n << " " << d << " " << nd << std::endl;
    typedef
    itk::Statistics::KdTreeWeightedCentroidNonterminalNode< TKDSample >
    _TNode;
    _TNode* node = dynamic_cast< _TNode* >( root );
    if( node != NULL )
    {
    // Get centroids
    typename _TNode::CentroidType left, right;
    if( root->Left( ) != NULL )
    root->Left( )->GetCentroid( left );
    if( root->Right( ) != NULL )
    root->Right( )->GetCentroid( right );

    } // fi
  */
  /*
    if( root->Left( ) != NULL )
    this->_Dummy( root->Left( ) );
    if( root->Right( ) != NULL )
    this->_Dummy( root->Right( ) );
  */
  /*
    if( root->IsTerminal( ) )
    {
    }
    else
    {
    if( root->Left( ) != NULL )
    this->_Dummy( root->Left( ) );
    if( root->Right( ) != NULL )
    this->_Dummy( root->Right( ) );
    } // fi
  */

}

// -------------------------------------------------------------------------
template< class M, class S >
cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
FindClosestPointInMesh( )
  : Superclass( ),
    m_BucketSize( 16 )
{
}

// -------------------------------------------------------------------------
template< class M, class S >
cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
~FindClosestPointInMesh( )
{
}

// -------------------------------------------------------------------------
template< class M, class S >
template< class V, class N, class P >
S cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
_LinePointDistance( const P& p0, const V& d, const V& n, const N& nd )
{
  V v0;
  for( unsigned int i = 0; i < Self::Dimension; ++i )
    v0[ i ] = N( p0[ i ] );
  return( S( ( itk::CrossProduct( v0, d ) - n ).GetNorm( ) / nd ) );
}

#endif // __CPM__ALGORITHMS__BASE__FINDCLOSESTPOINTINMESH__HXX__

// eof - $RCSfile$