#include "point_tris_dist.h"

#include "project_surface_points_on_poly.h"

#include <vtkUnstructuredGrid.h>
#include <vtkPointData.h>
#include <vtkCellData.h>
#include <vtkDoubleArray.h>
#include <vtkCellIterator.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkPolyData.h>
#include <vtkKdTree.h>
#include <vtkStaticCellLinks.h>

#include <limits>


vtkStandardNewMacro(ProjectSurfacePointsOnPoly);


ProjectSurfacePointsOnPoly::ProjectSurfacePointsOnPoly() {
	SetNumberOfInputPorts(2);
}


int ProjectSurfacePointsOnPoly::FillInputPortInformation(int port, vtkInformation *info) {
	if (port == 0) {
		vtkUnstructuredGridAlgorithm::FillInputPortInformation(port, info);
	} else if (port == 1) {
		vtkNew<vtkInformation> input2;
		input2->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData");
		info->Append(input2);
	}
	return 1;
}


vtkTypeBool ProjectSurfacePointsOnPoly::RequestData(
	vtkInformation *request,
	vtkInformationVector **inputVector,
	vtkInformationVector *outputVector) {
	(void) request;
	vtkUnstructuredGrid* us = vtkUnstructuredGrid::GetData(inputVector[0]);
	vtkPolyData* pd = vtkPolyData::GetData(inputVector[1]);
	vtkUnstructuredGrid* output =
		vtkUnstructuredGrid::GetData(outputVector);

	output->CopyStructure(us);
	output->GetPointData()->PassData(us->GetPointData());
	output->GetCellData()->PassData(us->GetCellData());

	/* Generate cell links, these tell us which cell a point belongs to. */
	vtkNew<vtkStaticCellLinks> links;
	links->BuildLinks(pd);

	vtkNew<vtkKdTree> kdTree;
	kdTree->BuildLocatorFromPoints(pd->GetPoints());

	vtkIdTypeArray *externalPoints =
		vtkIdTypeArray::SafeDownCast(us->GetFieldData()->GetArray("external_points"));
	for (vtkIdType i = 0; i < externalPoints->GetNumberOfTuples(); i++) {
		double p[3];
		us->GetPoint(externalPoints->GetTuple1(i), p);
		double dist2;
		vtkIdType closest = kdTree->FindClosestPoint(p, dist2);
		vtkIdType *cellIds = links->GetCells(closest);
		vtkIdType nCells = links->GetNcells(closest);
		double min_dist = std::numeric_limits<double>::infinity();
		double min_direction[3];

		/* For each tri the closest point belongs to. */
		for (vtkIdType j = 0; j < nCells; j++) {
			vtkIdType cellId = cellIds[j];
			double direction[3];
			double dist = pointTriangleDistance(p, pd->GetCell(cellId), direction);
			/* Find the closest one. */
			if (dist < min_dist) {
				min_dist = dist;
				min_direction[0] = direction[0];
				min_direction[1] = direction[1];
				min_direction[2] = direction[2];
			}
		}

		vtkMath::MultiplyScalar(min_direction, min_dist);
		vtkMath::Subtract(p, min_direction, p);
		us->GetPoints()->SetPoint(externalPoints->GetTuple1(i), p);
		us->Modified();
	}

	return true;
}