/*=========================================================================

  Program:   Visualization Toolkit
  Module:	vtkImplicitPolyDataDistance2.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

	 This software is distributed WITHOUT ANY WARRANTY; without even
	 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
	 PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkImplicitPolyDataDistance2.h"

#include "StartVTKIncludes.h"
#include "vtkCellData.h"
#include "vtkCellLocator.h"
#include "vtkGenericCell.h"
#include "vtkMath.h"
#include "vtkOBBTree.h"
#include "vtkObjectFactory.h"
#include "vtkPolyData.h"
#include "vtkPolygon.h"
#include "vtkTriangleFilter.h"
#include "vtkSmartPointer.h"
#include "EndVTKIncludes.h"

vtkStandardNewMacro(vtkImplicitPolyDataDistance2);

//-----------------------------------------------------------------------------
vtkImplicitPolyDataDistance2::vtkImplicitPolyDataDistance2()
{
  this->NoValue = 0.0;

  this->NoGradient[0] = 0.0;
  this->NoGradient[1] = 0.0;
  this->NoGradient[2] = 1.0;

  this->Input = NULL;
  this->Locator = NULL;
  this->Tolerance = 1e-12;
  this->Tree = NULL;
}

//-----------------------------------------------------------------------------
void vtkImplicitPolyDataDistance2::SetInput(vtkPolyData* input)
{
  if ( this->Input != input )
	{
	// Use a vtkTriangleFilter on the polydata input.
	// This is done to filter out lines and vertices to leave only
	// polygons which are required by this algorithm for cell normals.
	vtkSmartPointer<vtkTriangleFilter> triangleFilter =
	  vtkSmartPointer<vtkTriangleFilter>::New();
	triangleFilter->PassVertsOff();
	triangleFilter->PassLinesOff();

	triangleFilter->SetInputData( input );
	triangleFilter->Update();

	this->Input = triangleFilter->GetOutput();

	this->Input->BuildLinks();
	this->NoValue = this->Input->GetLength();

	if (this->Locator != NULL)
	  {
	  this->Locator->Delete();
	  }
	this->Locator = vtkCellLocator::New();
	this->Locator->SetDataSet(this->Input);
	this->Locator->SetTolerance(this->Tolerance);
	this->Locator->SetNumberOfCellsPerBucket(10);
	this->Locator->CacheCellBoundsOn();
	this->Locator->AutomaticOn();
	this->Locator->BuildLocator();

	if(this->Tree != NULL)
		this->Tree->Delete();
	this->Tree = vtkOBBTree::New();
	this->Tree->SetDataSet(this->Input);
	this->Tree->SetTolerance(this->Tolerance);
	this->Tree->BuildLocator();

	}

}

//-----------------------------------------------------------------------------
unsigned long vtkImplicitPolyDataDistance2::GetMTime()
{
  unsigned long mTime=this->vtkImplicitFunction::GetMTime();

  if ( this->Input != NULL )
	{
	unsigned long InputMTime = this->Input->GetMTime();
	mTime = (InputMTime > mTime ? InputMTime : mTime);
	}

  return mTime;
}

//-----------------------------------------------------------------------------
vtkImplicitPolyDataDistance2::~vtkImplicitPolyDataDistance2()
{
  if (this->Locator != NULL)
	{
	this->Locator->Delete();
	}

  if(this->Tree != NULL)
	  this->Tree->Delete();
}

//-----------------------------------------------------------------------------
double vtkImplicitPolyDataDistance2::EvaluateFunction(double x[3])
{
  double n[3] = {0, 0, 0};
  return this->SharedEvaluate(x, n); // get distance value returned, normal not used
}

//-----------------------------------------------------------------------------
void vtkImplicitPolyDataDistance2::EvaluateGradient(double x[3], double n[3])
{
  this->SharedEvaluate(x, n);	// get normal, returned distance value not used
}

//-----------------------------------------------------------------------------
double vtkImplicitPolyDataDistance2::SharedEvaluate(double x[3], double n[3])
{
	double ret = this->NoValue;

	for(int i=0 ; i < 3 ; i++)
		n[i] = this->NoGradient[i];

	// See if data set with polygons has been specified
	if (this->Input == NULL || Input->GetNumberOfCells() == 0)
	{
		vtkErrorMacro(<<"No polygons to evaluate function!");
		return ret;
	}

	double p[3];
	vtkIdType cellId;
	int subId;
	double vlen2;
	vtkDataArray* cnorms = 0;

	if ( this->Input->GetCellData() && this->Input->GetCellData()->GetNormals() )
		cnorms = this->Input->GetCellData()->GetNormals();

	// Get point id of closest point in data set.
	vtkSmartPointer<vtkGenericCell> cell = vtkSmartPointer<vtkGenericCell>::New();
	this->Locator->FindClosestPoint(x, p, cell, cellId, subId, vlen2);

	if (cellId != -1)	// point located
	{
		// dist = | point - x |
		ret = sqrt(vlen2);

		// grad = (point - x) / dist
		for (int i = 0; i < 3; i++)
			n[i] = (p[i] - x[i]) / (ret == 0. ? 1. : ret);

		double dist2, weights[3], pcoords[3], awnorm[3] = {0, 0, 0};;
		double closestPoint[3];
		cell->EvaluatePosition(p, closestPoint, subId, pcoords, dist2, weights);
		vtkIdList* idList = vtkIdList::New();
		int count = 0;

		for (int i = 0; i < 3; i++)
			count += (fabs(weights[i]) < this->Tolerance ? 1 : 0);

		// if weights contains no 0s
		if ( count == 0 || count == 1 )
		{
			// Compute face normal.
			// For count == 0, this is all we need.
			// For count = 1, we'll add in the normals from adjacent faces.
			if ( cnorms )
				cnorms->GetTuple(cellId, awnorm);
			else vtkPolygon::ComputeNormal(cell->Points, awnorm);
		}

		// if weights contains 1 0s
		if ( count == 1 )
		{
			// ... edge ... get two adjacent faces, compute average normal
			int a = -1, b = -1;
			for ( int edge = 0; edge < 3; edge++ )
			{
				if ( fabs(weights[edge]) < this->Tolerance )
				{
					a = cell->PointIds->GetId((edge + 1) % 3);
					b = cell->PointIds->GetId((edge + 2) % 3);
					break;
				}
			}

			if ( a == -1 )
			{
				vtkErrorMacro( << "Could not find edge when closest point is expected to be on an edge." );
				return this->NoValue;
			}

			this->Input->GetCellEdgeNeighbors(0, a, b, idList);
			for (int i = 0; i < idList->GetNumberOfIds(); i++)
			{
				double norm[3];
				if (cnorms)
					cnorms->GetTuple(idList->GetId(i), norm);
				else vtkPolygon::ComputeNormal(this->Input->GetCell(idList->GetId(i))->GetPoints(), norm);

				awnorm[0] += norm[0];
				awnorm[1] += norm[1];
				awnorm[2] += norm[2];
			}
			vtkMath::Normalize(awnorm);
		}

		// If weights contains 2 0s
		else if ( count == 2 )
		{
			// ... vertex ... this is the expensive case, get all adjacent
			// faces and compute sum(a_i * n_i) Angle-Weighted Pseudo
			// Normals, J. Andreas Baerentzen and Henrik Aanaes
			int a = -1;
			for (int i = 0; i < 3; i++)
			{
				if ( fabs( weights[i] ) > this->Tolerance )
					a = cell->PointIds->GetId(i);
			}

			if ( a == -1 )
			{
				vtkErrorMacro( << "Could not find point when closest point is expected to be a point." );
				return this->NoValue;
			}

			this->Input->GetPointCells(a, idList);
			for (int i = 0; i < idList->GetNumberOfIds(); i++)
			{
				double norm[3];
				if ( cnorms )
					cnorms->GetTuple(idList->GetId(i), norm);
				else vtkPolygon::ComputeNormal(this->Input->GetCell(idList->GetId(i))->GetPoints(), norm);

				// Compute angle at point a
				int b = this->Input->GetCell(idList->GetId(i))->GetPointId(0);
				int c = this->Input->GetCell(idList->GetId(i))->GetPointId(1);

				if (a == b)
					b = this->Input->GetCell(idList->GetId(i))->GetPointId(2);
				else if (a == c)
					c = this->Input->GetCell(idList->GetId(i))->GetPointId(2);

				double pa[3], pb[3], pc[3];
				this->Input->GetPoint(a, pa);
				this->Input->GetPoint(b, pb);
				this->Input->GetPoint(c, pc);
				for (int j = 0; j < 3; j++)
				{
					pb[j] -= pa[j];
					pc[j] -= pa[j];
				}

				vtkMath::Normalize(pb);
				vtkMath::Normalize(pc);
				double alpha = acos(vtkMath::Dot(pb, pc));
				awnorm[0] += alpha * norm[0];
				awnorm[1] += alpha * norm[1];
				awnorm[2] += alpha * norm[2];
			}
			vtkMath::Normalize(awnorm);
		}
		idList->Delete();

		// sign(dist) = dot(grad, cell normal)
		if (ret == 0)
			for (int i = 0; i < 3; i++)
				n[i] = awnorm[i];

		ret *= (vtkMath::Dot(n, awnorm) < 0.) ? 1. : -1.;

		if (ret > 0.)
			for (int i = 0; i < 3; i++)
				n[i] = -n[i];

		//Check bounds
		double bounds[6] = {0, 0, 0, 0, 0, 0};
		this->Input->GetBounds(bounds);

		if(x[0] < bounds[0] || x[0] > bounds[1] || x[1] < bounds[2] || x[1] > bounds[3] || x[2] < bounds[4] || x[2] > bounds[5])
			ret = fabs(ret);

		//Check inside and outside for all inside points
		if(ret < 0)
			ret = this->Tree->InsideOrOutside(x) * fabs(ret);
	}

	return ret;
}

//-----------------------------------------------------------------------------
void vtkImplicitPolyDataDistance2::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkImplicitFunction::PrintSelf(os,indent);

  os << indent << "NoValue: " << this->NoValue << "\n";
  os << indent << "NoGradient: (" << this->NoGradient[0] << ", "
	 << this->NoGradient[1] << ", " << this->NoGradient[2] << ")\n";
  os << indent << "Tolerance: " << this->Tolerance << "\n";

  if (this->Input)
	{
	os << indent << "Input : " << this->Input << "\n";
	}
  else
	{
	os << indent << "Input : (none)\n";
	}
}
