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

  Program:   Visualization Toolkit
  Module:    vtkClosestPointStrategy.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 "vtkClosestPointStrategy.h"

#include "vtkAbstractPointLocator.h"
#include "vtkCell.h"
#include "vtkGenericCell.h"
#include "vtkIdList.h"
#include "vtkObjectFactory.h"
#include "vtkPointSet.h"

#include <set>

//----------------------------------------------------------------------------
vtkStandardNewMacro(vtkClosestPointStrategy);

//----------------------------------------------------------------------------
vtkClosestPointStrategy::vtkClosestPointStrategy()
{
  // Preallocate for performance
  this->PointIds = vtkIdList::New();
  this->PointIds->Allocate(16);
  this->Neighbors = vtkIdList::New();
  this->Neighbors->Allocate(32);
  this->CellIds = vtkIdList::New();
  this->CellIds->Allocate(32);
  this->NearPointIds = vtkIdList::New();
  this->NearPointIds->Allocate(32);

  // You may ask, why this OwnsLocator rigamarole. The reason is because the
  // reference counting garbage collecter gets confused when the locator,
  // point set, and strategy are all mixed together; resulting in memory
  // leaks etc.
  this->OwnsLocator = false;
  this->PointLocator = nullptr;
}

//----------------------------------------------------------------------------
vtkClosestPointStrategy::~vtkClosestPointStrategy()
{
  this->PointIds->Delete();
  this->Neighbors->Delete();
  this->CellIds->Delete();
  this->NearPointIds->Delete();

  if (this->OwnsLocator && this->PointLocator != nullptr)
  {
    this->PointLocator->Delete();
    this->PointLocator = nullptr;
  }
}

//-----------------------------------------------------------------------------
void vtkClosestPointStrategy::SetPointLocator(vtkAbstractPointLocator* pL)
{
  if (pL != this->PointLocator)
  {
    if (this->PointLocator != nullptr && this->OwnsLocator)
    {
      this->PointLocator->Delete();
    }

    this->PointLocator = pL;

    if (pL != nullptr)
    {
      pL->Register(this);
    }

    this->OwnsLocator = true;
    this->Modified();
  }
}

//-----------------------------------------------------------------------------
int vtkClosestPointStrategy::Initialize(vtkPointSet* ps)
{
  // See whether anything has changed. If not, just return.
  if (this->PointSet != nullptr && ps == this->PointSet && this->MTime < this->InitializeTime)
  {
    return 1;
  }

  // Set up the point set; return on failure.
  if (this->Superclass::Initialize(ps) == 0)
  {
    return 0;
  }

  // Use the point set's point locator preferentially. If no point locator,
  // then we need to create one. If one is specified here in the strategy,
  // use that. If not, then used the point set's default build point locator
  // method.
  vtkAbstractPointLocator* psPL = ps->GetPointLocator();
  if (psPL == nullptr)
  {
    if (this->PointLocator != nullptr && this->OwnsLocator)
    {
      this->PointLocator->SetDataSet(ps);
      this->PointLocator->BuildLocator();
    }
    else
    {
      ps->BuildPointLocator();
      psPL = ps->GetPointLocator();
      this->PointLocator = psPL;
      this->OwnsLocator = false;
    }
  }
  else
  {
    this->PointLocator = psPL;
    this->OwnsLocator = false;
  }

  this->InitializeTime.Modified();

  return 1;
}

//-----------------------------------------------------------------------------
namespace
{

//-----------------------------------------------------------------------------
// Used internally by FindCell to walk through neighbors from a starting cell.
// The arguments are the same as those for FindCell.  In addition, visitedCells
// keeps a list of cells already traversed.  If we run into such already
// visited, the walk terminates since we assume we already walked from that cell
// and found nothing.  The ptIds and neighbors lists are buffers used
// internally.  They are passed in so that they do not have to be continuously
// reallocated.
vtkIdType FindCellWalk(vtkClosestPointStrategy* self, vtkPointSet* ps, double x[3], vtkCell* cell,
  vtkGenericCell* gencell, vtkIdType cellId, double tol2, int& subId, double pcoords[3],
  double* weights, std::set<vtkIdType>& visitedCells, vtkIdList* ptIds, vtkIdList* neighbors)
{
  const int VTK_MAX_WALK = 12;
  for (int walk = 0; walk < VTK_MAX_WALK; walk++)
  {
    // Check to see if we already visited this cell.
    if (visitedCells.find(cellId) != visitedCells.end())
      break;
    visitedCells.insert(cellId);

    // Get information for the cell.
    cell = self->SelectCell(ps, cellId, cell, gencell);

    // Check to see if the current, cached cell contains the point.
    double closestPoint[3];
    double dist2;
    if ((cell->EvaluatePosition(x, closestPoint, subId, pcoords, dist2, weights) == 1) &&
      (dist2 <= tol2))
    {
      return cellId;
    }

    // This is not the right cell.  Find the next one.
    cell->CellBoundary(subId, pcoords, ptIds);
    ps->GetCellNeighbors(cellId, ptIds, neighbors);
    // If there is no next one, exit.
    if (neighbors->GetNumberOfIds() < 1)
      break;
    // Set the next cell as the current one and iterate.
    cellId = neighbors->GetId(0);
    cell = nullptr;
  }

  // Could not find a cell.
  return -1;
}

//-----------------------------------------------------------------------------
vtkIdType FindCellWalk(vtkClosestPointStrategy* self, vtkPointSet* ps, double x[3],
  vtkGenericCell* gencell, vtkIdList* cellIds, double tol2, int& subId, double pcoords[3],
  double* weights, std::set<vtkIdType>& visitedCells, vtkIdList* ptIds, vtkIdList* neighbors)
{
  vtkIdType numCellIds = cellIds->GetNumberOfIds();
  for (vtkIdType i = 0; i < numCellIds; i++)
  {
    vtkIdType cellId = cellIds->GetId(i);
    vtkIdType foundCell = FindCellWalk(self, ps, x, nullptr, gencell, cellId, tol2, subId, pcoords,
      weights, visitedCells, ptIds, neighbors);
    if (foundCell >= 0)
    {
      return foundCell;
    }
  }
  return -1;
}

} // anonymous namespace

//-----------------------------------------------------------------------------
vtkIdType vtkClosestPointStrategy::FindCell(double x[3], vtkCell* cell, vtkGenericCell* gencell,
  vtkIdType cellId, double tol2, int& subId, double pcoords[3], double* weights)
{
  // Check to see if the point is within the bounds of the data.  This is not
  // a strict check, but it is fast.
  const double* bounds = this->Bounds;
  double tol = sqrt(tol2);
  if ((x[0] < bounds[0] - tol) || (x[0] > bounds[1] + tol) || (x[1] < bounds[2] - tol) ||
    (x[1] > bounds[3] + tol) || (x[2] < bounds[4] - tol) || (x[2] > bounds[5] + tol))
  {
    return -1;
  }

  // Implement the strategy proper
  this->VisitedCells.clear();

  // If we are given a starting cell, try that.
  vtkIdType foundCell;
  if (cell && (cellId >= 0))
  {
    foundCell = FindCellWalk(this, this->PointSet, x, cell, gencell, cellId, tol2, subId, pcoords,
      weights, this->VisitedCells, this->PointIds, this->Neighbors);
    if (foundCell >= 0)
    {
      return foundCell;
    }
  }

  // The starting cell didn't work, find the point closest to the coordinates
  // given and search the attached cells.
  vtkIdType ptId = this->PointLocator->FindClosestPoint(x);
  if (ptId < 0)
    return -1;
  this->PointSet->GetPointCells(ptId, this->CellIds);
  foundCell = FindCellWalk(this, this->PointSet, x, gencell, this->CellIds, tol2, subId, pcoords,
    weights, this->VisitedCells, this->PointIds, this->Neighbors);
  if (foundCell >= 0)
  {
    return foundCell;
  }

  // It is possible that the toplogy is not fully connected as points are
  // coincident.  Handle this by looking at every point within the tolerance
  // and consider all cells connected.  It has been suggested that we should
  // really do this coincident point check at every point as we walk through
  // neighbors, which would happen in FindCellWalk.  If that were ever
  // implemented, this step might become unnecessary.
  double ptCoord[3];
  this->PointSet->GetPoint(ptId, ptCoord);
  this->PointLocator->FindPointsWithinRadius(sqrt(tol2), ptCoord, this->NearPointIds);
  this->NearPointIds->DeleteId(ptId); // Already searched this one.
  vtkIdType i, numPts = this->NearPointIds->GetNumberOfIds();
  for (i = 0; i < numPts; i++)
  {
    this->PointSet->GetPointCells(this->NearPointIds->GetId(i), this->CellIds);
    foundCell = FindCellWalk(this, this->PointSet, x, gencell, this->CellIds, tol2, subId, pcoords,
      weights, this->VisitedCells, this->PointIds, this->Neighbors);
    if (foundCell >= 0)
    {
      return foundCell;
    }
  }

  // Could not find a containing cell. Either the query point is outside of
  // the dataset, or there is an uncommon pathology of disconnected cells and
  // points (if using a point locator approach). In this latter case, a cell
  // locator is necessary.
  return -1;
}

//----------------------------------------------------------------------------
void vtkClosestPointStrategy::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "PointLocator: " << this->PointLocator << "\n";
}