#include "vtkCellLocator.h"
#include "vtkSTLReader.h"
#include "vtkSTLWriter.h"
#include "vtkPolyData.h"
#include "vtkPolyDataReader.h"
#include "vtkPolyDataNormals.h"
#include "vtkThinPlateSplineTransform.h"
#include "vtkTransformPolyDataFilter.h"
#include "vtkPointDataToCellData.h"
#include "vtkUnstructuredGrid.h"
#include "vtkFloatArray.h"
#include "vtkIdList.h"
#include "vtkPoints.h"
#include "vtkMath.h"

#include <fstream.h>
#include <iostream.h>
#include <stdlib.h>

// Program file name: closestpoint_diff_unsigned.cxx
// Purpose: compute (unsigned) distance between two surfaces (per cell), and output to color file.
// Author: Tron Darvann
// Date: March 11, 2006

int main( int argc, char *argv[] )
{
  if (argc != 6) {
    cout << "Usage: target.vtk source.vtk colorfile.clr max_dist min_dist" << endl;
    cout << "Inputs: target.vtk, source.vtk" << endl;
    cout << "Output: color (.clr) file with distances/colors for source.vtk " << endl;
    cout << "max_dist min_dist: range for the coloring" << endl;
    cout << "Performs: for each cell on source, locate closest point on target; compute and write distance to file." << endl;
    cout << "Limitation: input must contain triangles only." << endl;
    return -1;
  } 
 
  vtkIdType cellId;
  vtkIdList *cells = vtkIdList::New();
  vtkPoints *SourcePoints;
  int i, j, subId, pId, ncells;
  float d, c[3], m[3], sign;
  double p0[3], p1[3], p2[3];
  double n[3], u[3], v[3], dv[3], ln, ldv;
  double dotp, t, argu;
  
  // Read the Target surface.
  vtkPolyDataReader *treader = vtkPolyDataReader::New();
    treader->SetFileName(argv[1]);
    treader->Update();
    vtkPolyData *Target = treader->GetOutput();

  // Read the Source surface.
  vtkPolyDataReader *sreader = vtkPolyDataReader::New();
    sreader->SetFileName(argv[2]);
    sreader->Update();
    vtkPolyData *Source = sreader->GetOutput();

  cout << argv[4] << " " << argv[5] << endl;
  //cout << "Done Reading" << endl;

  // Create the Cell Locator.
  vtkCellLocator *cellLocator = vtkCellLocator::New();
    cellLocator->SetDataSet(Target);
    cellLocator->BuildLocator();

  // Put source points in SourcePoints
  SourcePoints = Source->GetPoints();

  // Get the number of cells in the Source.
  ncells = Source->GetNumberOfCells();

  // Open the output color file.
  ofstream out;
  out.open(argv[3], ios::out);
  out << "# LANDMARKER SURFACE SCALAR COLOR FILE V1" << endl;
  out << argv[4] << " " << argv[5] << endl;
  out << "rainbow" << endl;

  // For each cell i do:
  for (i = 0; i < ncells; i++) {
     // Get the points belonging to this cell.
     Source->GetCellPoints(i,cells);
          
     // Put the three vertex coordinates in p0, p1, p2.
     pId = cells->GetId(0);

     SourcePoints->GetPoint(pId,p0);
     pId = cells->GetId(1);
     SourcePoints->GetPoint(pId,p1);
     pId = cells->GetId(2);
     SourcePoints->GetPoint(pId,p2);
 
     // Compute the cell center m.
     m[0] = (p0[0] + p1[0] + p2[0]) / 3.0;
     m[1] = (p0[1] + p1[1] + p2[1]) / 3.0; 
     m[2] = (p0[2] + p1[2] + p2[2]) / 3.0; 

     // Compute closest point c and the distance d.
     cellLocator->FindClosestPoint(m, c, cellId, subId, d);
     d = sqrt(d);

     // Compute two vectors u and v in the cell.
     u[0] = p1[0] -  p0[0];
     u[1] = p1[1] -  p0[1];
     u[2] = p1[2] -  p0[2];
     v[0] = p2[0] -  p0[0];
     v[1] = p2[1] -  p0[1];
     v[2] = p2[2] -  p0[2];

     // Compute the cell non-normalized normal.
     n[0] = u[1] * v[2] - u[2] * v[1];
     n[1] = u[2] * v[0] - u[0] * v[2];
     n[2] = u[0] * v[1] - u[1] * v[0];

     // Compute closest point c and the distance d.
     cellLocator->FindClosestPoint(m, c, cellId, subId, d);
     d = sqrt(d);

     // Compute displacement vector dv.
     dv[0] = c[0] - m[0];
     dv[1] = c[1] - m[1];
     dv[2] = c[2] - m[2];

     // Compute angle t between displacement vecor and cell normal.
     dotp = dv[0] * n[0] + dv[1] * n[1] + dv[2] * n[2];
     // Compute the length of n.
     ln = sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);
     // Compute the length of dv.
     ldv = sqrt(dv[0] * dv[0] + dv[1] * dv[1] + dv[2] * dv[2]);
     argu = dotp / (ln * ldv);
     t = acos(argu);

     // Determine the sign.
     sign = -1.0;
     if(t > 1.5707963) {
        sign = 1.0;
     }

     // Apply sign to distance.
     d = d * sign;


     
     //cout << "m" << m[0] << endl;
     //cout << "c" << c[0] << endl;
     //cout << "d" << d << endl;
     // Write to color file.
     out << d << endl;

  }
  // Close the output file.
  out.close();

}