/*
g++ test.cxx -I/usr/include/vtk-5.10/ -L/usr/lib64/ -lvtkFiltering -lvtkCommon -lvtkIO -otest
*/

#include <iostream>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkPolygon.h>
#include <vtkIdList.h>
#include <vtkMath.h>
#include <vtkPlane.h>

void ComputeNormal (vtkPoints *pts, vtkIdList *poly, double* n) {

    n[0] = 0; n[1] = 0; n[2] = 0;
    double pt0[3], pt1[3];

    pts->GetPoint(poly->GetId(0), pt0);

    unsigned int nbr = poly->GetNumberOfIds();

    for (unsigned int i = 0; i < nbr; i++) 
      {
      pts->GetPoint(poly->GetId((i+1)%nbr), pt1);

      n[0] += (pt0[1]-pt1[1])*(pt0[2]+pt1[2]);
      n[1] += (pt0[2]-pt1[2])*(pt0[0]+pt1[0]);
      n[2] += (pt0[0]-pt1[0])*(pt0[1]+pt1[1]);

      pt0[0] = pt1[0];
      pt0[1] = pt1[1];
      pt0[2] = pt1[2];
      }

    vtkMath::Normalize(n);

    // Okay now let's see if this plane is actually planar.
    // Using the normal computed, loop over all points and
    // compute distance from the plane.
    double *dist = new double [nbr];
    pts->GetPoint(poly->GetId(0), pt0);

    //Loop over all points and see what there distance is from the plane
    for (unsigned int i = 0; i < nbr; i++) 
      {
      pts->GetPoint(poly->GetId(i), pt1);
      dist[i] = vtkPlane::DistanceToPlane(pt1, n, pt0);
      }

    delete [] dist;
}

int main () {
    vtkPolyDataReader *r = vtkPolyDataReader::New();
    r->SetFileName("a.vtk");
    r->Update();

    vtkPolyData *pd = r->GetOutput();

    double m[3], n[3];

    vtkIdList *poly = vtkIdList::New();

    pd->GetCellPoints(0, poly);

    vtkPolygon::ComputeNormal(pd->GetPoints(), poly->GetNumberOfIds(), poly->GetPointer(0), m);

    ComputeNormal(pd->GetPoints(), poly, n);

    std::cout << m[0] << ", " << m[1] << ", " << m[2] << std::endl;
    std::cout << n[0] << ", " << n[1] << ", " << n[2] << std::endl;

    double d1 = vtkMath::Dot(m, pd->GetPoints()->GetPoint(0));
    double d2 = vtkMath::Dot(n, pd->GetPoints()->GetPoint(0));

    double t1 = vtkMath::Dot(m, pd->GetPoints()->GetPoint(1))-d1;
    double t2 = vtkMath::Dot(n, pd->GetPoints()->GetPoint(1))-d2;

    std::cout << t1 << std::endl;
    std::cout << t2 << std::endl;

    vtkPolyData *pd2 = vtkPolyData::New();
    pd2->SetPoints(pd->GetPoints());
    pd2->Allocate(1);
    pd2->InsertNextCell(VTK_POLY_LINE, poly);

    vtkPolyDataWriter *w = vtkPolyDataWriter::New();
    w->SetInputData(pd2);
    w->SetFileName("b.vtk");
    w->Update();

    w->Delete();
    poly->Delete();
    r->Delete();

    return 0;

}