#include "mesh_fit_filter.h"

#include <vtkUnstructuredGrid.h>
#include <vtkPointData.h>
#include <vtkCellData.h>
#include <vtkDoubleArray.h>
#include <vtkCellIterator.h>

#include "kd_tree.h"

vtkStandardNewMacro(MeshFitFilter);

vtkTypeBool MeshFitFilter::RequestData(
	vtkInformation *request,
	vtkInformationVector **inputVector,
	vtkInformationVector *outputVector) {
	(void) request;

	vtkUnstructuredGrid* input =
		vtkUnstructuredGrid::GetData(inputVector[0]);
	vtkUnstructuredGrid* output =
		vtkUnstructuredGrid::GetData(outputVector);
    output->CopyStructure(input);
	output->GetPointData()->PassData(input->GetPointData());


    std::vector<KdTree::Tuple> points;

	auto it = input->NewCellIterator();
	for(it->InitTraversal(); !it->IsDoneWithTraversal(); it->GoToNextCell()) {
		if(it->GetCellType() != VTK_TETRA) continue;
		
        vtkIdList *idList = it->GetPointIds();
        
        for(int i = 0; i < 4; ++i) {
            vtkIdType id = idList->GetId(i);

            double point[3];
            input->GetPoint(id, point);

            points.push_back({point, id});

            //std::cout << "[" << point[0] << ", " << point[1] << ", " << point[2] << "] (" << id << ")\n";
        }
	}

    KdTree kdTree;
    kdTree.fill(points);

    std::cout << "[0.3, 1.5, -0.1] => " << kdTree.query({0.3, 1.5, -0.1}) << std::endl;
    std::cout << "[5.1, 1.1, 0.5] => " << kdTree.query({5.1, 1.1, 0.5}) << std::endl;
    std::cout << "[0, 0, 1] => " << kdTree.query({0, 0, 1}) << std::endl;

	return true;
}
