/*
 * CloudViewerCellPicker.cpp
 *
 *  Created on: Aug 21, 2018
 *      Author: mathieu
 */

#include "rtabmap/gui/CloudViewerCellPicker.h"

#include <vtkImageData.h>
#include <vtkRenderer.h>
#include <vtkAbstractPicker.h>
#include <vtkPicker.h>
#include <vtkAbstractCellLocator.h>
#include <vtkIdList.h>
#include <vtkCellPicker.h>
#include <vtkLODProp3D.h>
#include <vtkMapper.h>
#include <vtkGenericCell.h>
#include <vtkMath.h>
#include <vtkTexture.h>
#include <vtkObjectFactory.h>
#include <vtkSmartPointer.h>
#include <vtkPoints.h>
#include <vtkProperty.h>

namespace rtabmap {

// Standard VTK macro for *New ()
vtkStandardNewMacro (CloudViewerCellPicker);

CloudViewerCellPicker::CloudViewerCellPicker()
{
	cell_ = vtkGenericCell::New();
	pointIds_ = vtkIdList::New();
}

CloudViewerCellPicker::~CloudViewerCellPicker()
{
	cell_->Delete();
	pointIds_->Delete();
}

double CloudViewerCellPicker::IntersectActorWithLine(const double p1[3],
		const double p2[3],
		double t1, double t2,
		double tol,
		vtkProp3D *prop,
		vtkMapper *mapper)
{
	// This code was taken from the original CellPicker with almost no
	// modification except for the locator and texture additions.

	// Intersect each cell with ray.  Keep track of one closest to
	// the eye (within the tolerance tol) and within the clipping range).
	// Note that we fudge the "closest to" (tMin+this->Tolerance) a little and
	// keep track of the cell with the best pick based on parametric
	// coordinate (pick the minimum, maximum parametric distance). This
	// breaks ties in a reasonable way when cells are the same distance
	// from the eye (like cells laying on a 2D plane).

	vtkDataSet *data = mapper->GetInput();
	double tMin = VTK_DOUBLE_MAX;
	double minPCoords[3];
	double pDistMin = VTK_DOUBLE_MAX;
	vtkIdType minCellId = -1;
	int minSubId = -1;
	double minXYZ[3];
	minXYZ[0] = minXYZ[1] = minXYZ[2] = 0.0;
	double ray[3] = {p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]};
	vtkMath::Normalize(ray);
	vtkActor * actor = vtkActor::SafeDownCast(prop);

	// Polydata has no 3D cells
	int isPolyData = data->IsA("vtkPolyData");

	vtkCollectionSimpleIterator iter;
	vtkAbstractCellLocator *locator = 0;
	this->Locators->InitTraversal(iter);
	while ( (locator = static_cast<vtkAbstractCellLocator *>(
			this->Locators->GetNextItemAsObject(iter))) )
	{
		if (locator->GetDataSet() == data)
		{
			break;
		}
	}

	// Make a new p1 and p2 using the clipped t1 and t2
	double q1[3], q2[3];
	q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
	q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
	if (t1 != 0.0 || t2 != 1.0)
	{
		for (int j = 0; j < 3; j++)
		{
			q1[j] = p1[j]*(1.0 - t1) + p2[j]*t1;
			q2[j] = p1[j]*(1.0 - t2) + p2[j]*t2;
		}
	}

	// Use the locator if one exists for this data
	if (locator)
	{
		vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
		vtkSmartPointer<vtkIdList> intersectCells = vtkSmartPointer<vtkIdList>::New();

		locator->IntersectWithLine(q1, q2, intersectPoints, intersectCells);
		for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
		{
			double intersection[3];
			intersectPoints->GetPoint(i, intersection);
		}

		if (!locator->IntersectWithLine(q1, q2, tol, tMin, minXYZ,
				minPCoords, minSubId, minCellId,
				this->cell_))
		{
			return VTK_DOUBLE_MAX;
		}

		// Stretch tMin out to the original range
		if (t1 != 0.0 || t2 != 1.0)
		{
			tMin = t1*(1.0 - tMin) + t2*tMin;
		}

		// If cell is a strip, then replace cell with a sub-cell
		this->SubCellFromCell(this->cell_, minSubId);
	}
	else
	{
		vtkIdList *pointIds = this->pointIds_;
		vtkIdType numCells = data->GetNumberOfCells();

		for (vtkIdType cellId = 0; cellId < numCells; cellId++)
		{
			double t;
			double x[3];
			double pcoords[3];
			pcoords[0] = pcoords[1] = pcoords[2] = 0;
			int newSubId = -1;
			int numSubIds = 1;

			// If it is a strip, we need to iterate over the subIds
			int cellType = data->GetCellType(cellId);
			int useSubCells = this->HasSubCells(cellType);
			if (useSubCells)
			{
				// Get the pointIds for the strip and the length of the strip
				data->GetCellPoints(cellId, pointIds);
				numSubIds = this->GetNumberOfSubCells(pointIds, cellType);
			}

			// This will only loop once unless we need to deal with a strip
			for (int subId = 0; subId < numSubIds; subId++)
			{
				if (useSubCells)
				{
					// Get a sub-cell from a the strip
					this->GetSubCell(data, pointIds, subId, cellType, this->cell_);
				}
				else
				{
					data->GetCell(cellId, this->cell_);
				}

				int cellPicked = 0;
				if (isPolyData)
				{
					// Polydata can always be picked with original endpoints
					cellPicked = this->cell_->IntersectWithLine(
							const_cast<double *>(p1), const_cast<double *>(p2),
							tol, t, x, pcoords, newSubId);
				}
				else
				{
					// Any 3D cells need to be intersected with a line segment that
					// has been clipped with the clipping planes, in case one end is
					// actually inside the cell.
					cellPicked = this->cell_->IntersectWithLine(
							q1, q2, tol, t, x, pcoords, newSubId);

					// Stretch t out to the original range
					if (t1 != 0.0 || t2 != 1.0)
					{
						t = t1*(1.0 - t) + t2*t;
					}
				}

				if (cellPicked && t <= (tMin + this->Tolerance) && t >= t1 && t <= t2)
				{
					double pDist = this->cell_->GetParametricDistance(pcoords);
					if (pDist < pDistMin || (pDist == pDistMin && t < tMin))
					{
////////////////////////////////////////////////////////////////////////////////////
// BEGIN: Modifications from VTK 6.2
////////////////////////////////////////////////////////////////////////////////////
						bool visible = true;
						if(actor->GetProperty()->GetBackfaceCulling() ||
						   actor->GetProperty()->GetFrontfaceCulling())
						{
							// Get the cell weights
							vtkIdType numPoints = this->cell_->GetNumberOfPoints();
							double *weights = new double[numPoints];
							for (vtkIdType i = 0; i < numPoints; i++)
							{
								weights[i] = 0;
							}

							// Get the interpolation weights (point is thrown away)
							double point[3] = {0.0,0.0,0.0};
							this->cell_->EvaluateLocation(minSubId, minPCoords, point, weights);

							double normal[3] = {0.0,0.0,0.0};

							if (this->ComputeSurfaceNormal(data, this->cell_, weights, normal))
							{
								if(actor->GetProperty()->GetBackfaceCulling())
								{
									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] <= 0;
								}
								else
								{
									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] >= 0;
								}
							}
							delete [] weights;
						}
						if(visible)
						{
							tMin = t;
							pDistMin = pDist;
							// save all of these
							minCellId = cellId;
							minSubId = newSubId;
							if (useSubCells)
							{
								minSubId = subId;
							}
							for (int k = 0; k < 3; k++)
							{
								minXYZ[k] = x[k];
								minPCoords[k] = pcoords[k];
							}
						}
////////////////////////////////////////////////////////////////////////////////////
// END: Modifications from VTK 6.2
////////////////////////////////////////////////////////////////////////////////////
					} // for all subIds
				} // if minimum, maximum
			} // if a close cell
		} // for all cells
	}

	// Do this if a cell was intersected
	if (minCellId >= 0 && tMin < this->GlobalTMin)
	{
		this->ResetPickInfo();

		// Get the cell, convert to triangle if it is a strip
		vtkGenericCell *cell = this->cell_;

		// If we used a locator, we already have the picked cell
		if (!locator)
		{
			int cellType = data->GetCellType(minCellId);

			if (this->HasSubCells(cellType))
			{
				data->GetCellPoints(minCellId, this->pointIds_);
				this->GetSubCell(data, this->pointIds_, minSubId, cellType, cell);
			}
			else
			{
				data->GetCell(minCellId, cell);
			}
		}

		// Get the cell weights
		vtkIdType numPoints = cell->GetNumberOfPoints();
		double *weights = new double[numPoints];
		for (vtkIdType i = 0; i < numPoints; i++)
		{
			weights[i] = 0;
		}

		// Get the interpolation weights (point is thrown away)
		double point[3];
		cell->EvaluateLocation(minSubId, minPCoords, point, weights);

		this->Mapper = mapper;

		// Get the texture from the actor or the LOD
		vtkActor *actor = 0;
		vtkLODProp3D *lodActor = 0;
		if ( (actor = vtkActor::SafeDownCast(prop)) )
		{
			this->Texture = actor->GetTexture();
		}
		else if ( (lodActor = vtkLODProp3D::SafeDownCast(prop)) )
		{
			int lodId = lodActor->GetPickLODID();
			lodActor->GetLODTexture(lodId, &this->Texture);
		}

		if (this->PickTextureData && this->Texture)
		{
			// Return the texture's image data to the user
			vtkImageData *image = this->Texture->GetInput();
			this->DataSet = image;

			// Get and check the image dimensions
			int extent[6];
			image->GetExtent(extent);
			int dimensionsAreValid = 1;
			int dimensions[3];
			for (int i = 0; i < 3; i++)
			{
				dimensions[i] = extent[2*i + 1] - extent[2*i] + 1;
				dimensionsAreValid = (dimensionsAreValid && dimensions[i] > 0);
			}

			// Use the texture coord to set the information
			double tcoord[3];
			if (dimensionsAreValid &&
					this->ComputeSurfaceTCoord(data, cell, weights, tcoord))
			{
				// Take the border into account when computing coordinates
				double x[3];
				x[0] = extent[0] + tcoord[0]*dimensions[0] - 0.5;
				x[1] = extent[2] + tcoord[1]*dimensions[1] - 0.5;
				x[2] = extent[4] + tcoord[2]*dimensions[2] - 0.5;

				this->SetImageDataPickInfo(x, extent);
			}
		}
		else
		{
			// Return the polydata to the user
			this->DataSet = data;
			this->CellId = minCellId;
			this->SubId = minSubId;
			this->PCoords[0] = minPCoords[0];
			this->PCoords[1] = minPCoords[1];
			this->PCoords[2] = minPCoords[2];

			// Find the point with the maximum weight
			double maxWeight = 0;
			vtkIdType iMaxWeight = -1;
			for (vtkIdType i = 0; i < numPoints; i++)
			{
				if (weights[i] > maxWeight)
				{
					iMaxWeight = i;
				}
			}

			// If maximum weight is found, use it to get the PointId
			if (iMaxWeight != -1)
			{
				this->PointId = cell->PointIds->GetId(iMaxWeight);
			}
		}

		// Set the mapper position
		this->MapperPosition[0] = minXYZ[0];
		this->MapperPosition[1] = minXYZ[1];
		this->MapperPosition[2] = minXYZ[2];

		// Compute the normal
		if (!this->ComputeSurfaceNormal(data, cell, weights, this->MapperNormal))
		{
			// By default, the normal points back along view ray
			this->MapperNormal[0] = p1[0] - p2[0];
			this->MapperNormal[1] = p1[1] - p2[1];
			this->MapperNormal[2] = p1[2] - p2[2];
			vtkMath::Normalize(this->MapperNormal);
		}

		delete [] weights;
	}

	return tMin;
}

} /* namespace rtabmap */