#include <Experiments/SkeletonHausdorffDistance.h>
#include <cpInstances/DataObjects/Image.h>
#include <cpInstances/Mesh.h>
#include <cpInstances/DataObjects/Skeleton.h>
#include <cpExtensions/DataStructures/Skeleton.h>

#include <vtkImageData.h>
#include <vtkPolyData.h>

// -------------------------------------------------------------------------
fpaPluginsExperiments::SkeletonHausdorffDistance::
SkeletonHausdorffDistance( )
  : Superclass( )
{
  this->_ConfigureInput< cpInstances::DataObjects::Image >( "DistanceMap", true, false );
  this->_ConfigureInput< cpInstances::Mesh >( "Seeds", true, false );
  this->_ConfigureInput< cpInstances::Skeleton >( "Skeleton1", true, false );
  this->_ConfigureInput< cpInstances::Skeleton >( "Skeleton2", true, false );
}

// -------------------------------------------------------------------------
fpaPluginsExperiments::SkeletonHausdorffDistance::
~SkeletonHausdorffDistance( )
{
}

// -------------------------------------------------------------------------
void fpaPluginsExperiments::SkeletonHausdorffDistance::
_GenerateData( )
{
  typedef cpExtensions::DataStructures::Skeleton< 3 > _TSkeleton;

  auto dmap = this->GetInputData< vtkImageData >( "DistanceMap" );
  auto seeds = this->GetInputData< vtkPolyData >( "Seeds" );
  auto sk1 = this->GetInputData< _TSkeleton >( "Skeleton1" );
  auto sk2 = this->GetInputData< _TSkeleton >( "Skeleton2" );

  double buf[ 3 ], pcoords[ 3 ];
  seeds->GetPoint( 0, buf );
  int ijk[ 3 ];
  dmap->ComputeStructuredCoordinates( buf, ijk, pcoords );
  double radius =
    dmap->GetScalarComponentAsDouble( ijk[ 0 ], ijk[ 1 ], ijk[ 2 ], 0 );

  double d1 = this->_Distance( sk1, sk2, NULL, 0 );
  double d2 = this->_Distance( sk1, sk2, buf, radius );

  std::cout
    << std::endl
    << "-------------------------------" << std::endl
    << "D1    : " << d1 << std::endl
    << "D2    : " << d2 << std::endl
    << "Radius: " << radius << std::endl
    << "Seed  : " << buf[ 0 ] << " " << buf[ 1 ] << " " << buf[ 2 ] << std::endl
    << "-------------------------------" << std::endl;
}

// -------------------------------------------------------------------------
template< class _TSkeleton >
double fpaPluginsExperiments::SkeletonHausdorffDistance::
_Distance( _TSkeleton* sk1, _TSkeleton* sk2, double* center, double radius )
{
  auto lst1 = this->_PointList( sk1, center, radius );
  auto lst2 = this->_PointList( sk2, center, radius );
  double dist = -std::numeric_limits< double >::max( );
  typename _TSkeleton::TPath::TPoint point;
  for( auto p1 : lst1 )
  {
    double ldist = std::numeric_limits< double >::max( );
    typename _TSkeleton::TPath::TPoint lpoint;
    for( auto p2 : lst2 )
    {
      double d = p1.EuclideanDistanceTo( p2 );
      if( d < ldist )
      {
        ldist = d;
        lpoint = p2;

      } // fi

    } // rof
    if( ldist > dist )
    {
      dist = ldist;
      point = lpoint;

    } // fi

  } // rof
  return( dist );
}

// -------------------------------------------------------------------------
template< class _TSkeleton >
std::vector< typename _TSkeleton::TPath::TPoint >
fpaPluginsExperiments::SkeletonHausdorffDistance::
_PointList( _TSkeleton* sk, double* center, double radius )
{
  typename _TSkeleton::TPath::TPoint p_center;
  if( center != NULL )
    for( unsigned int d = 0; d < _TSkeleton::Dimension; ++d )
      p_center[ d ] = center[ d ];

  std::vector< typename _TSkeleton::TPath::TPoint > lst;
  auto mIt = sk->BeginEdgesRows( );
  for( ; mIt != sk->EndEdgesRows( ); ++mIt )
  {
    auto rIt = mIt->second.begin( );
    for( ; rIt != mIt->second.end( ); ++rIt )
    {
      auto eIt = rIt->second.begin( );
      for( ; eIt != rIt->second.end( ); ++eIt )
      {
        auto path = *eIt;
        for( unsigned int i = 0; i < path->GetSize( ); ++i )
        {
          auto p = path->GetPoint( i );
          if( center != NULL )
          {
            if( p_center.EuclideanDistanceTo( p ) > radius )
              lst.push_back( p );
          }
          else
            lst.push_back( p );

        } // rof

      } // rof

    } // rof

  } // rof
  return( lst );
}

// eof - $RCSfile$