#include "vtkCGALPoisson.h"

// VTK related includes
#include "vtkCellArrayIterator.h"
#include "vtkDataSet.h"
#include "vtkIdList.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"

// CGAL related includes
#include <CGAL/poisson_surface_reconstruction.h>

vtkStandardNewMacro(vtkCGALPoisson);

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

//------------------------------------------------------------------------------
int vtkCGALPoisson::RequestData(
  vtkInformation*, vtkInformationVector** inputVector, vtkInformationVector* outputVector)
{

  // using Triangle = std::array<std::size_t, 3>;
  using CGAL_PM = std::pair<CGAL_Point, CGAL_Vector>;

  // Get the input and output data objects.
  vtkPolyData* input  = vtkPolyData::GetData(inputVector[0]);
  vtkPolyData* output = vtkPolyData::GetData(outputVector);

  // Create the point cloud with normals
  // -----------------------------------

  // TODO ?
  // vtkDataArray* normals = this->GetInputArrayToProcess(0, inputVector);
  vtkDataArray* normals = input->GetPointData()->GetArray("Normals");
  if (normals == nullptr || normals->GetNumberOfComponents() != 3)
  {
    vtkErrorMacro("Invalid normals on point cloud:" << normals);
    return 0;
  }

  vtkIdType nbPts = input->GetNumberOfPoints();

  std::vector<CGAL_PM> points;
  points.reserve(nbPts);
  for (vtkIdType i = 0; i < nbPts; i++)
  {
    auto pt   = input->GetPoint(i);
    auto norm = normals->GetTuple3(i);
    points.emplace_back(CGAL_Point(pt[0], pt[1], pt[2]), CGAL_Vector(norm[0], norm[1], norm[2]));
  }

  // CGAL Processing
  // ---------------

  CGAL_Polyhedron cgalMesh;

  try
  {

    double average_spacing = CGAL::compute_average_spacing<CGAL::Sequential_tag>(
      points, 6, CGAL::parameters::point_map(CGAL::First_of_pair_property_map<CGAL_PM>()));

    CGAL::poisson_surface_reconstruction_delaunay(points.begin(), points.end(),
      CGAL::First_of_pair_property_map<CGAL_PM>(), CGAL::Second_of_pair_property_map<CGAL_PM>(),
      cgalMesh, average_spacing);
  }
  catch (std::exception& e)
  {
    vtkErrorMacro("CGAL Exception: " << e.what());
    return 0;
  }

  // VTK Output
  // ----------

  // points (vertices in surfaceMesh are not contiguous)
  vtkNew<vtkPoints> pts;
  const vtkIdType   outNPts = num_vertices(cgalMesh);
  pts->Allocate(outNPts);
  std::map<CGAL_Point, vtkIdType> vmap;

  for (auto vertex : vertices(cgalMesh))
  {
    vtkIdType id = pts->InsertNextPoint(vertex->point()[0], vertex->point()[1], vertex->point()[2]);
    vmap[vertex->point()] = id;
  }
  pts->Squeeze();

  // cells
  vtkNew<vtkCellArray> cells;
  cells->AllocateEstimate(num_faces(cgalMesh), 3);

  for (auto face : faces(cgalMesh))
  {
    vtkNew<vtkIdList> ids;
    for (auto edge : halfedges_around_face(halfedge(face, cgalMesh), cgalMesh))
    {
      auto s = source(edge, cgalMesh);
      ids->InsertNextId(vmap[s->point()]);
    }
    cells->InsertNextCell(ids);
  }
  cells->Squeeze();

  // VTK dataset
  output->SetPoints(pts);
  output->SetPolys(cells);

  return 1;
}
