#include <vtk/vtkActor.h>
#include <vtk/vtkCleanPolyData.h>
#include <vtk/vtkCubeSource.h>
#include <vtk/vtkPolyData.h>
#include <vtk/vtkPolyDataMapper.h>
#include <vtk/vtkProperty.h>
#include <vtk/vtkRenderer.h>
#include <vtk/vtkRenderWindow.h>
#include <vtk/vtkRenderWindowInteractor.h>
#include <vtk/vtkSmartPointer.h>

#define MY_SP(class, variable)\
   vtkSmartPointer<class> variable = vtkSmartPointer<class>::New();

const double cst_dRadius = 1.2;

#include <vtk/vtkKdTreePointLocator.h>
#include <vtk/vtkPointLocator.h>

bool ComputePointDistances(vtkKdTreePointLocator *locator, vtkPolyData *in)
{
   int num = in->GetNumberOfPoints();
   cout << "number of input points : " << num << endl;
   vtkPoints *pPoints = in->GetPoints();
   double pt[3];
   int cpt = 0;
   double dDist2 = 0;

   int bad = 0;
   int id = -1;
   for(int i=0; i<num; ++i)
   {
      pPoints->GetPoint(i, pt);
      id = locator->FindClosestPointWithinRadius(cst_dRadius, pt, dDist2);
      if(-1 != id)
      {
         if(sqrt(dDist2) > cst_dRadius)
            ++bad;
         ++cpt;
      }
   }
   cout << "should not be tested : " << bad << endl;
   cout << "number of distances computed : " << cpt << endl;

   return true;
}
bool ComputePointDistances(vtkPointLocator *locator, vtkPolyData *in)
{
   int num = in->GetNumberOfPoints();
   cout << "number of input points : " << num << endl;
   vtkPoints *pPoints = in->GetPoints();
   double pt[3];
   int cpt = 0;
   double dDist2 = 0;

   int bad = 0;
   int id = -1;
   for(int i=0; i<num; ++i)
   {
      pPoints->GetPoint(i, pt);
      id = locator->FindClosestPointWithinRadius(cst_dRadius, pt, dDist2);
      if(-1 != id)
      {
         if(sqrt(dDist2) > cst_dRadius)
            ++bad;
         ++cpt;
      }
   }
   cout << "should not be tested : " << bad << endl;
   cout << "number of distances computed : " << cpt << endl;

   return true;
}

int main(int argc, char* argv[])
{
   /////////////////////////////////////////////////////////
   MY_SP(vtkRenderer, ren1);
   ren1->SetBackground(0.2, 0.2, 0.2);
   MY_SP(vtkRenderWindow, renWin);
   renWin->SetSize( 512, 512 );
   renWin->AddRenderer(ren1);
   MY_SP(vtkRenderWindowInteractor, iren);
   iren->SetRenderWindow(renWin);

   /////////////////////////////////////////////////////////
   MY_SP(vtkCubeSource, src);
   src->SetBounds(-1,1,-1,1,-1,1);
   src->Update();

   MY_SP(vtkCleanPolyData, clean);
   clean->SetInputConnection(src->GetOutputPort());
   clean->Update();
   vtkPolyData *cube = clean->GetOutput();

   MY_SP(vtkPolyDataMapper, Map);
   Map->SetInput(cube);

   MY_SP(vtkActor, act);
   act->SetMapper(Map);
   act->GetProperty()->SetInterpolationToFlat();
   act->GetProperty()->SetColor(1.0, 0.0, 0.0);

   ren1->AddActor(act);


   MY_SP(vtkCubeSource, src2);
   src2->SetBounds(0,2,0,2,0,2);
   src2->Update();

   MY_SP(vtkCleanPolyData, clean2);
   clean2->SetInputConnection(src2->GetOutputPort());
   clean2->Update();
   vtkPolyData *cube2 = clean2->GetOutput();

   MY_SP(vtkPolyDataMapper, Map2);
   Map2->SetInput(cube2);

   MY_SP(vtkActor, act2);
   act2->SetMapper(Map2);
   act2->GetProperty()->SetInterpolationToFlat();
   act2->GetProperty()->SetColor(0.0, 1.0, 0.0);

   ren1->AddActor(act2);

   /////////////////////////////////////////////////////////
   MY_SP(vtkPointLocator, locator);
   locator->SetTolerance(0.0);
   locator->SetDataSet(cube2);
   locator->BuildLocator();

   ComputePointDistances(locator, cube);

   MY_SP(vtkKdTreePointLocator, locator2);
   locator2->SetTolerance(0.0);
   locator2->SetDataSet(cube2);
   locator2->BuildLocator();

   ComputePointDistances(locator2, cube);

   /////////////////////////////////////////////////////////
   ren1->ResetCamera();
   renWin->Render();
   iren->Start();

   /////////////////////////////////////////////////////////
   return 0;
}