#include "closest_polymesh_point.h"
#include "point_tris_dist.h"
#include <limits>


vtkIdType findClosestPoint(const double *point, vtkPointSet *pointSet) {
	vtkIdType minPoint = 0;
	double minDist2 = std::numeric_limits<double>::infinity();
	for (vtkIdType i = 0; i < pointSet->GetNumberOfPoints(); i++) {
		double point2[3];
		pointSet->GetPoint(i, point2);
		double dist2 = vtkMath::Distance2BetweenPoints(point, point2);
		if (dist2 < minDist2) {
			minDist2 = dist2;
			minPoint = i;
		}
	}
	return minPoint;
}


void closestPolyMeshPoint(vtkPolyData *polyMesh,
                          const double *point,
                          vtkKdTree *kdTree,
                          vtkStaticCellLinks *links,
                          double *toClosestPoint,
                          double *distanceToClosestPoint) {
	vtkIdType closestPolyMeshPoint;
	{
		double dummy;
		// closestPolyMeshPoint = kdTree->FindClosestPoint((double *) point, dummy);
		closestPolyMeshPoint = findClosestPoint(point, polyMesh);
	}
	vtkIdType *cellIds = links->GetCells(closestPolyMeshPoint);
	vtkIdType nCells = links->GetNcells(closestPolyMeshPoint);
	if (nCells == 0) {
		*distanceToClosestPoint = -1;
		return;
	}
	double minDist = std::numeric_limits<double>::infinity();

	/* For each tri the closest point belongs to. */
	for (vtkIdType i = 0; i < nCells; i++) {
		vtkIdType cellId = cellIds[i];
		double direction[3];
		double dist = pointTriangleDistance(
			(double *) point, polyMesh->GetCell(cellId), direction);
		/* Find the closest one. */
		if (dist < minDist) {
			minDist = dist;
			toClosestPoint[0] = direction[0];
			toClosestPoint[1] = direction[1];
			toClosestPoint[2] = direction[2];
		}
	}
	vtkMath::MultiplyScalar(toClosestPoint, minDist);
	*distanceToClosestPoint = minDist;
}