#include <getopt.h>

#include <vtkVersion.h>
#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkUnstructuredGrid.h>
#include <vtkCamera.h>
#include <vtkCellArray.h>
#include <vtkClipDataSet.h>
#include <vtkLagrangeTriangle.h>
#include <vtkDataSetSurfaceFilter.h>
#include <vtkDoubleArray.h>
#include <vtkPlane.h>
#include <vtkPointData.h>
#include <vtkPointLocator.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkTessellatorFilter.h>
#include <vtkXMLPolyDataWriter.h>
#include <vtkXMLUnstructuredGridReader.h>
#include <vtkXMLUnstructuredGridWriter.h>

namespace
{
void Identity(const double* pIn, double* pOut)
{
  for (int i=0;i<3;i++)
    {
    pOut[i] = pIn[i];
    }
}

void Paraboloid(const double* pIn, double* pOut)
{
  double dist = 0.;
  for (int i=0;i<3;i++)
    {
    dist += pIn[i]*pIn[i];
    }
  dist = sqrt(dist)/sqrt(2.);

  pOut[0] = pIn[0];
  pOut[1] = pIn[1];
  pOut[2] = pIn[2] - 1. + 2.*dist*dist;
}

void Sinusoid(const double* pIn, double* pOut)
{
  double dist = 0.;
  for (int i=0;i<3;i++)
    {
    dist += pIn[i]*pIn[i];
    }
  dist = 2.*M_PI*sqrt(dist)/sqrt(2.);

  pOut[0] = pIn[0];
  pOut[1] = pIn[1];
  pOut[2] = pIn[2] + .5*cos(2.*dist);
}

void AddTriangle(int nPoints,
                 const double* p0,
                 const double* p1,
                 const double* p2,
                 vtkPointLocator* pointLocator,
                 vtkCellArray* cells,
                 void (*map)(const double*,double*)=&Identity)
{
  vtkSmartPointer<vtkLagrangeTriangle> t =
    vtkSmartPointer<vtkLagrangeTriangle>::New();

  t->GetPointIds()->SetNumberOfIds(nPoints);
  t->GetPoints()->SetNumberOfPoints(nPoints);
  t->Initialize();
  int order = t->GetOrder();

  double pIn[3], pOut[3];
  vtkIdType pId,bIndex[3];
  for (vtkIdType i=0;i<nPoints;i++)
    {
      vtkLagrangeTriangle::BarycentricIndex(i,bIndex,order);
    for (vtkIdType j=0;j<3;j++)
      {
      pIn[j] = (p0[j]*bIndex[2])/order +
        (p1[j]*bIndex[0])/order + (p2[j]*bIndex[1])/order;
      }
    map(pIn,pOut);
    pointLocator->InsertUniquePoint(pOut, pId);
    t->GetPointIds()->SetId(i,pId);
    }
  cells->InsertNextCell(t);
}

}

int main(int argc, char** argv)
{
  std::string usage =
    "\n"
    "Usage: VisualizeLagrangeTriangleMesh <options>\n"
    "\n"
    "This program models a rectangular surface using a Lagrange triangle mesh.\n"
    "\n"
    "\tAvailable options:\n"
    "\t -h, --help               (shows this message and exits)\n"
    "\t -o, --order              (Lagrange interpolation order)\n"
    "\t -x, --x-disc             (# of x divisions)\n"
    "\t -y, --y-disc             (# of y divisions)\n"
    "\t -m, --mapping            (point mapping from parametric to xyz.\n"
    "\t                             0: identity\n"
    "\t                             1: paraboloid)\n"
    "\t                             2: sinusoid)\n"
    ;

  int order = 4;
  vtkIdType nX = 4;
  vtkIdType nY = 4;
  void (*mapping)(const double*,double*) = &Identity;

  static struct option longOptions[] = {
    {"help", no_argument, 0, 'h'},
    {"order", required_argument, 0, 'o'},
    {"x-disc", required_argument, 0, 'x'},
    {"y-disc", required_argument, 0, 'y'},
    {"mapping", required_argument, 0, 'm'},
  };

  static const char *optString = "ho:x:y:m:";

  while(1) {
    char optId = getopt_long(argc, argv,optString, longOptions, NULL);
    if(optId == -1) break;
    switch(optId) {
    case('h'):
    std::cout<<usage<<std::endl;
      return 0;
    case('o'):
      order = atoi(optarg);
      break;
    case('x'):
      nX = atoi(optarg);
      break;
    case('y'):
      nY = atoi(optarg);
      break;
    case('m'):
      if (atoi(optarg) == 1)
        {
        mapping = &Paraboloid;
        }
      else if (atoi(optarg) == 2)
        {
        mapping = &Sinusoid;
        }
      break;
    default:
      std::cout<<usage<<std::endl;
      return 1;
    }
  }

  vtkSmartPointer<vtkUnstructuredGrid> unstructuredGrid =
    vtkSmartPointer<vtkUnstructuredGrid>::New();

  vtkSmartPointer<vtkPoints> pointArray =
    vtkSmartPointer<vtkPoints>::New();

  vtkSmartPointer<vtkPointLocator> pointLocator =
    vtkSmartPointer<vtkPointLocator>::New();
  double bounds[6] = {-1.,1.,-1.,1.,-1.,1.};
  pointLocator->InitPointInsertion(pointArray,bounds);

  vtkSmartPointer<vtkCellArray> cellArray =
    vtkSmartPointer<vtkCellArray>::New();

  int nPointsPerTriangle = (order+1)*(order+2)/2;

  double p[4][3];
  double dx = (bounds[1] - bounds[0])/nX;
  double dy = (bounds[3] - bounds[2])/nY;
  for (vtkIdType i=0;i<4;i++)
    {
    for (vtkIdType j=0;j<2;j++)
      {
      p[i][j] = bounds[2*j];
      }
    p[i][2] = 0.;
    }
  p[1][0] += dx;
  p[2][0] += dx;
  p[2][1] += dy;
  p[3][1] += dy;

  for (vtkIdType xInc = 0; xInc < nX; xInc++)
    {
    p[0][1] = p[1][1] = bounds[2];
    p[2][1] = p[3][1] = bounds[2] + dy;

    for (vtkIdType yInc = 0; yInc < nY; yInc++)
      {
        if (xInc < nX/2 == yInc < nY/2)
          {
          AddTriangle(nPointsPerTriangle,p[0],p[1],p[3],
                      pointLocator, cellArray, mapping);
          AddTriangle(nPointsPerTriangle,p[1],p[2],p[3],
                      pointLocator, cellArray, mapping);
          }
        else
          {
          AddTriangle(nPointsPerTriangle,p[0],p[1],p[2],
                      pointLocator, cellArray, mapping);
          AddTriangle(nPointsPerTriangle,p[0],p[2],p[3],
                      pointLocator, cellArray, mapping);
          }
        for (vtkIdType i=0;i<4;i++)
          {
          p[i][1] += dy;
          }
      }

    p[0][0] = p[3][0] = p[1][0];
    p[1][0] += dx;
    p[2][0] += dx;
    }

  unstructuredGrid->SetPoints(pointArray);
  unstructuredGrid->SetCells(VTK_LAGRANGE_TRIANGLE, cellArray);

  vtkIdType nPoints = unstructuredGrid->GetPoints()->GetNumberOfPoints();

  vtkSmartPointer<vtkDoubleArray> radiant =
    vtkSmartPointer<vtkDoubleArray>::New();
  radiant->SetName("Distance from Origin");
  radiant->SetNumberOfTuples(nPoints);

  vtkSmartPointer<vtkDoubleArray> elevation =
    vtkSmartPointer<vtkDoubleArray>::New();
  elevation->SetName("Elevation");
  elevation->SetNumberOfTuples(nPoints);


  double maxDist = 0;
  for (vtkIdType i = 0; i < nPoints; i++)
    {
    double xyz[3];
    unstructuredGrid->GetPoints()->GetPoint(i,xyz);
    double dist = sqrt(xyz[0]*xyz[0] + xyz[1]*xyz[1] + xyz[2]*xyz[2]);
    maxDist = (dist > maxDist ? dist : maxDist);
    radiant->SetTypedTuple(i, &dist);
    elevation->SetTypedTuple(i, &xyz[2]);
    }

  unstructuredGrid->GetPointData()->AddArray(radiant);
  unstructuredGrid->GetPointData()->AddArray(elevation);
  unstructuredGrid->GetPointData()->SetScalars(radiant);

  vtkSmartPointer<vtkXMLUnstructuredGridWriter> writer =
    vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
  writer->SetFileName("test.vtu");
  writer->SetInputData(unstructuredGrid);
  writer->Write();

  vtkSmartPointer<vtkXMLUnstructuredGridReader> reader =
    vtkSmartPointer<vtkXMLUnstructuredGridReader>::New();
  reader->SetFileName("test.vtu");

  vtkSmartPointer<vtkTessellatorFilter> tessellate =
    vtkSmartPointer<vtkTessellatorFilter>::New();
  tessellate->SetInputConnection(reader->GetOutputPort());

  vtkSmartPointer<vtkXMLUnstructuredGridWriter> writer2 =
    vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
  writer2->SetFileName("test2.vtu");
  writer2->SetInputConnection(tessellate->GetOutputPort());
  writer2->Write();

  vtkSmartPointer<vtkDataSetSurfaceFilter> surfaceFilter =
    vtkSmartPointer<vtkDataSetSurfaceFilter>::New();
  // surfaceFilter->SetInputConnection(tessellate->GetOutputPort());
  surfaceFilter->SetInputConnection(reader->GetOutputPort());
  surfaceFilter->Update();
  vtkPolyData* polydata = surfaceFilter->GetOutput();

  vtkSmartPointer<vtkXMLPolyDataWriter> pWriter =
    vtkSmartPointer<vtkXMLPolyDataWriter>::New();
  pWriter->SetFileName("test.vtp");
  pWriter->SetInputData(polydata);
  pWriter->Write();

  std::cout << "Output has " << polydata->GetNumberOfPoints() << " points." << std::endl;

  // Visualize
  vtkSmartPointer<vtkPolyDataMapper> mapper =
    vtkSmartPointer<vtkPolyDataMapper>::New();
  mapper->SetInputData(polydata);
  mapper->SetScalarRange(maxDist*.5,maxDist);

  vtkSmartPointer<vtkActor> actor =
    vtkSmartPointer<vtkActor>::New();
  actor->SetMapper(mapper);

  vtkSmartPointer<vtkCamera> camera =
    vtkSmartPointer<vtkCamera>::New();
  // camera->SetPosition(-2.*maxDist, -2.*maxDist, -2.*maxDist);
  camera->SetPosition(.5,.5, -2.*maxDist);
  camera->SetFocalPoint(.5, .5, 0.);

  vtkSmartPointer<vtkRenderer> renderer =
    vtkSmartPointer<vtkRenderer>::New();
  renderer->SetActiveCamera(camera);

   vtkSmartPointer<vtkRenderWindow> renderWindow =
    vtkSmartPointer<vtkRenderWindow>::New();
  renderWindow->AddRenderer(renderer);
  vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
    vtkSmartPointer<vtkRenderWindowInteractor>::New();
  renderWindowInteractor->SetRenderWindow(renderWindow);

  renderer->AddActor(actor);

  renderWindow->Render();
  renderWindowInteractor->Start();

  return EXIT_SUCCESS;
}