
#include "createMesh3D.h"

#include "itkLinearInterpolateImageFunction.h"

#include "vtkCellArray.h"
#include "vtkCellType.h"
#include "vtkDelaunay3D.h"
#include "vtkIdList.h"
#include "vtkSmartPointer.h"

#include "vtkQhullDelaunay3D.h"

#include "MersenneTwisterRNG.h"

vtkSmartPointer<vtkUnstructuredGrid>
  createMesh3D(
    vtkPoints* points, const itk::Image<float, 3>* distImg, bool useQHull)
{
  // Jiggle points to avoid degenerate tetras
  vtkSmartPointer<vtkPoints> jiggledPoints =
    vtkSmartPointer<vtkPoints>::New();
  jiggledPoints->DeepCopy(points);

  MersenneTwisterRNG* rng = MersenneTwisterRNG::GetGlobalInstance();

  double x[3];
  double y[3];

  for (unsigned int k = 0; k < jiggledPoints->GetNumberOfPoints(); k++)
  {
    points->GetPoint(k, x);

    for (unsigned int dim = 0; dim < 3; dim++)
    {
      double r = 2.0*rng->GenerateUniformRealOpenInterval() - 1.0;
      x[dim] += r * 0.05;
    }

    jiggledPoints->SetPoint(k, x);
  }

  vtkSmartPointer<vtkUnstructuredGrid> mesh;

  if (useQHull)
  {
    mesh = vtkQhullDelaunay3D(jiggledPoints);
  }
  else
  {
    vtkSmartPointer<vtkUnstructuredGrid> tempMesh =
      vtkSmartPointer<vtkUnstructuredGrid>::New();
    tempMesh->SetPoints(jiggledPoints);

    vtkSmartPointer<vtkDelaunay3D> delaunay =
      vtkSmartPointer<vtkDelaunay3D>::New();
    delaunay->SetAlpha(0.0);
    //delaunay->SetTolerance(0.5);
    delaunay->SetTolerance(0.0);
    delaunay->SetOffset(3.0);

    delaunay->SetInput(tempMesh);
    delaunay->Update();

    mesh = delaunay->GetOutput();
  }

  vtkSmartPointer<vtkUnstructuredGrid> outMesh = 
    vtkSmartPointer<vtkUnstructuredGrid>::New();
  outMesh->SetPoints(mesh->GetPoints());

  typedef itk::LinearInterpolateImageFunction<itk::Image<float, 3>, double>
    InterpolatorType;

  InterpolatorType::Pointer distInterp = InterpolatorType::New();
  distInterp->SetInputImage(distImg);

  //vtkSmartPointer<vtkCellArray> newcells =
  //  vtkSmartPointer<vtkCellArray>::New();
  //newcells->Allocate(mesh->GetNumberOfCells());

  unsigned int numCells = mesh->GetNumberOfCells();

  for (unsigned int el = 0; el < numCells; el++)
  {
    vtkSmartPointer<vtkIdList> ids = vtkSmartPointer<vtkIdList>::New();
    mesh->GetCellPoints(el, ids);

    unsigned int n = ids->GetNumberOfIds();

    for (unsigned int dim = 0; dim < 3; dim++)
      y[dim] = 0.0;

    for (unsigned int i = 0; i < n; i++)
    {
      points->GetPoint(ids->GetId(i), x);
      for (int dim = 0; dim < 3; dim++)
        y[dim] += x[dim];
    }

    for (unsigned int dim = 0; dim < 3; dim++)
      y[dim] /= n;

    itk::Image<float, 3>::PointType p;
    p[0] = y[0];
    p[1] = y[1];
    p[2] = y[2];

    if (!distInterp->IsInsideBuffer(p))
      continue;

    double phi = distInterp->Evaluate(p);

    // Don't insert convex hull triangulation
    if (phi > -0.1)
      continue;

    //newcells->InsertNextCell(n, ids->GetPointer(0));
    outMesh->InsertNextCell(VTK_TETRA, 4, ids->GetPointer(0));

  } // for el

  outMesh->BuildLinks();

  //mesh->SetCells(VTK_TETRA, newcells);

  return outMesh;

}
