/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestPointLocators.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkIdList.h"
#include "vtkKdTree.h"
#include "vtkKdTreePointLocator.h"
#include "vtkMath.h"
#include "vtkOctreePointLocator.h"
#include "vtkPointLocator.h"
#include "vtkPoints.h"
#include "vtkStructuredGrid.h"

// returns true if 2 points are equidistant from x, within a tolerance
bool ArePointsEquidistant(double x[3], vtkIdType id1, vtkIdType id2, vtkPointSet* grid)
{
  if (id1 == id2)
  {
    return true;
  }
  float firstDist2 = vtkMath::Distance2BetweenPoints(x, grid->GetPoint(id1));
  float secondDist2 = vtkMath::Distance2BetweenPoints(x, grid->GetPoint(id2));

  float differenceDist2 = firstDist2 - secondDist2;
  if (differenceDist2 < 0)
  {
    differenceDist2 = -differenceDist2;
  }

  if (differenceDist2 / (firstDist2 + secondDist2) > .00001)
  {
    cerr << "Results do not match (first dist2=" << firstDist2 << " , second dist2=" << secondDist2
         << ") ";
    return false;
  }
  return true;
}

// checks that every point in firstList has a matching point (based
// on distance) in secondList
bool DoesListHaveProperPoints(
  double x[3], vtkIdList* firstList, vtkIdList* secondList, vtkPointSet* grid)
{
  for (vtkIdType uid = 0; uid < firstList->GetNumberOfIds(); uid++)
  {
    int found = 0;
    for (vtkIdType kid = 0; kid < secondList->GetNumberOfIds(); kid++)
    {
      if (firstList->GetId(uid) == secondList->GetId(kid))
      {
        found = 1;
        break;
      }
    }
    if (!found)
    {
      for (vtkIdType kid = 0; kid < secondList->GetNumberOfIds(); kid++)
      {
        if (ArePointsEquidistant(x, firstList->GetId(uid), secondList->GetId(kid), grid))
        {
          found = 1;
          break;
        }
      }
    }
    if (!found)
    {
      return false;
    }
  }
  return true;
}

// This test compares results for different point locators since they should
// all return the same results (within a tolerance)
int ComparePointLocators(vtkAbstractPointLocator* locator1, vtkAbstractPointLocator* locator2)
{
  int rval = 0;
  int i, j, k, kOffset, jOffset, offset;
  float x[3];
  static const int dims[3] = { 39, 31, 31 };

  // Create the structured grid.
  vtkStructuredGrid* sgrid = vtkStructuredGrid::New();
  sgrid->SetDimensions(dims);

  // We also create the points.
  vtkPoints* points = vtkPoints::New();
  points->Allocate(dims[0] * dims[1] * dims[2]);

  for (k = 0; k < dims[2]; k++)
  {
    x[2] = 1.0 + k * 1.2;
    kOffset = k * dims[0] * dims[1];
    for (j = 0; j < dims[1]; j++)
    {
      x[1] = sqrt(10. + j * 2.);
      jOffset = j * dims[0];
      for (i = 0; i < dims[0]; i++)
      {
        x[0] = 1 + i * i * .5;
        offset = i + jOffset + kOffset;
        points->InsertPoint(offset, x);
      }
    }
  }
  sgrid->SetPoints(points);
  points->Delete();

  locator1->SetDataSet(sgrid);
  locator2->SetDataSet(sgrid);

  double bounds[6];
  sgrid->GetBounds(bounds);
  for (i = 0; i < 3; i++)
  {
    // expand the search so we are looking for points inside and outside the BB
    bounds[i * 2] *= .5;
    bounds[i * 2 + 1] *= 1.2;
  }
  int numSearchPoints = 20;
  vtkIdList* locator1List = vtkIdList::New();
  vtkIdList* locator2List = vtkIdList::New();
  for (i = 0; i < numSearchPoints; i++)
  {
    double point[3] = { (bounds[0] + (bounds[1] - bounds[0]) * i / numSearchPoints),
      (bounds[2] + (bounds[3] - bounds[2]) * i / numSearchPoints),
      (bounds[4] + (bounds[5] - bounds[4]) * i / numSearchPoints) };
    vtkIdType locator1Pt = locator1->FindClosestPoint(point);
    vtkIdType locator2Pt = locator2->FindClosestPoint(point);
    if (!ArePointsEquidistant(point, locator1Pt, locator2Pt, sgrid))
    {
      cerr << " from FindClosestPoint.\n";
      rval++;
    }
    int N = 1 + i * 250 / numSearchPoints; // test different amounts of points to search for
    locator1->FindClosestNPoints(N, point, locator1List);
    locator2->FindClosestNPoints(N, point, locator2List);
    if (!ArePointsEquidistant(point, locator1Pt, locator1List->GetId(0), sgrid))
    {
      cerr
        << "for comparing FindClosestPoint and first result of FindClosestNPoints for locator1.\n";
      rval++;
    }
    if (!ArePointsEquidistant(point, locator2Pt, locator2List->GetId(0), sgrid))
    {
      cerr
        << "for comparing FindClosestPoint and first result of FindClosestNPoints for locator2.\n";
      rval++;
    }

    for (j = 0; j < N; j++)
    {
      if (!ArePointsEquidistant(point, locator2List->GetId(j), locator1List->GetId(j), sgrid))
      {
        cerr << "for point " << j << " for ClosestNPoints search.\n";
        rval++;
      }
    }
    double radius = 10;
    locator1->FindPointsWithinRadius(radius, point, locator1List);
    locator2->FindPointsWithinRadius(radius, point, locator2List);
    if (!DoesListHaveProperPoints(point, locator1List, locator2List, sgrid) ||
      !DoesListHaveProperPoints(point, locator2List, locator1List, sgrid))
    {
      cerr << "Problem with FindPointsWithinRadius\n";
      rval++;
    }

    double dist2;
    locator1Pt = locator1->FindClosestPointWithinRadius(radius, point, dist2);
    locator2Pt = locator2->FindClosestPointWithinRadius(radius, point, dist2);
    if (locator1Pt < 0 || locator2Pt < 0)
    {
      if (locator1Pt >= 0 || locator2Pt >= 0)
      {
        cerr << "Inconsistent results for FindClosestPointWithinRadius\n";
        rval++;
      }
    }
    else if (!ArePointsEquidistant(point, locator1Pt, locator2Pt, sgrid))
    {
      cerr << "Incorrect result for FindClosestPointWithinRadius.\n";
      rval++;
    }
    if (locator1Pt >= 0)
    {
      locator1List->Reset();
      locator1List->InsertNextId(locator1Pt);
      if (!DoesListHaveProperPoints(point, locator1List, locator2List, sgrid))
      {
        cerr << "Inconsistent results for FindClosestPointWithinRadius and FindPointsWithRadius\n";
        rval++;
      }
    }
  }

  locator1List->Delete();
  locator2List->Delete();

  sgrid->Delete();

  return rval; // returns 0 if all tests passes
}

// This test does a brute force test on the KdTree point locator
// to make sure that at least one of the point locators used
// above gives a correct result for FindClosestPoint().
int TestKdTreePointLocator()
{
  int rval = 0;
  vtkIdType num_points = 1000;
  vtkIdType num_test_points = 100;

  vtkIdType idA;
  vtkIdType closest_id = -1;
  vtkIdType point;
  vtkIdType test_point;

  double pointA[3];
  double pointB[3];

  vtkPoints* A = vtkPoints::New();
  A->SetDataTypeToDouble();
  A->SetNumberOfPoints(num_points);
  for (point = 0; point < num_points; ++point)
  {
    pointA[0] = ((double)rand()) / RAND_MAX;
    pointA[1] = ((double)rand()) / RAND_MAX;
    pointA[2] = ((double)rand()) / RAND_MAX;
    A->SetPoint(point, pointA);
  }

  vtkKdTree* kd = vtkKdTree::New();
  kd->BuildLocatorFromPoints(A);

  for (test_point = 0; test_point < num_test_points; ++test_point)
  {
    double min_dist2 = 10.0;
    pointB[0] = ((double)rand()) / RAND_MAX;
    pointB[1] = ((double)rand()) / RAND_MAX;
    pointB[2] = ((double)rand()) / RAND_MAX;
    for (point = 0; point < num_points; ++point)
    {
      double dist2;
      double dx, dy, dz;
      A->GetPoint(point, pointA);
      dx = pointA[0] - pointB[0];
      dy = pointA[1] - pointB[1];
      dz = pointA[2] - pointB[2];
      dist2 = dx * dx + dy * dy + dz * dz;
      if (dist2 < min_dist2)
      {
        closest_id = point;
        min_dist2 = dist2;
      }
    }
    double ld2;
    idA = kd->FindClosestPoint(pointB, ld2);
    float diff = static_cast<float>(ld2) - static_cast<float>(min_dist2);
    if (ld2 == 0)
    {
      ld2 = 1; // avoid divide by zero error below
    }
    if ((idA != closest_id) && (diff / ld2 > .00001))
    {
      cerr << "KdTree found the closest point to be " << ld2
           << " away but a brute force method returned a closer distance of " << min_dist2 << endl;
      rval++;
    }
  }

  kd->Delete();
  A->Delete();

  return rval;
}

int TestPointLocators(int, char*[])
{
  vtkKdTreePointLocator* kdTreeLocator = vtkKdTreePointLocator::New();
  vtkPointLocator* uniformLocator = vtkPointLocator::New();

  cout << "Comparing vtkPointLocator to vtkKdTreePointLocator.\n";
  int rval = ComparePointLocators(uniformLocator, kdTreeLocator);

  vtkOctreePointLocator* octreeLocator = vtkOctreePointLocator::New();

  cout << "Comparing vtkOctreePointLocator to vtkKdTreePointLocator.\n";
  rval += ComparePointLocators(octreeLocator, kdTreeLocator);

  kdTreeLocator->Delete();
  uniformLocator->Delete();
  octreeLocator->Delete();

  rval += TestKdTreePointLocator();

  return rval;
}