//==============================================================================
/*! \file
 * OpenMesh Toolkit for mesh analysis    \n
 * Copyright (c) 2010 by Rostislav Hulik     \n
 *
 * Author:  Rostislav Hulik, rosta.hulik@gmail.com  \n
 * Date:    2010/11/03                          \n
 *
 * This file is part of software developed for support of Rostislav Hulik's dissertation thesis at dcgm-robotics@FIT group.
 *
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * Description:
 * - class computing curvatures on a mesh
 */

#ifndef _OM_CURVATURE_HXX_
#define _OM_CURVATURE_HXX_

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// constants for computations
const double M_PIHALF = M_PI/2;
const double M_TWOPI = M_PI*2;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Constructor - create an instance of this class 
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
OMCurvature<Mesh, Scalar>::OMCurvature(Mesh *mesh, OpenMesh::VPropHandleT<Normal> &curvVectorHandle, OpenMesh::VPropHandleT<Scalar> &curvMagHandle)
{
	m_mesh = mesh;
	m_mesh->request_face_normals();
	m_mesh->request_vertex_normals();
	m_mesh->update_normals();

	// initialize pre-computed voronoi area
	m_mesh->add_property(m_areas, "<VoronoiA>");
	Mesh::FIter end = m_mesh->faces_end();
	for (Mesh::FIter face = m_mesh->faces_begin(); face!= end; ++face)
		m_mesh->property(m_areas, face) = AreasVector(0.0, 0.0, 0.0);

	m_curvVectorHandle = curvVectorHandle;
	m_curvMagHandle = curvMagHandle;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Function returns principal curvature directions for specified vertex
// Computation from paper of Gabriel Taubin - Estimating the tenspr of curvature of a surface from a polyhedral approximation
// @param vertex Vertex handle
// @param max maximum curvature direction vector
// @param min minimum curvature direction vector
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
void OMCurvature<Mesh, Scalar>::getCurvatureDirections(VertexHandle vertex, Normal &max, Normal &min)
{
	// helper variables (for not allocate them in the loop)
	Normal normal = m_mesh->normal(vertex);
	Normal inVector;
	Normal outVector;
	HalfedgeHandle he;

	Scalar Kij;
	Scalar w;
	FaceHandle face1;
	FaceHandle face2;
	Matrix3x3 TijAux;
	ColumnVector3 Tij;
	RowVector3 TijT;

	// Helper normals in matrix structure
	ColumnVector3 normalVector;
	RowVector3 normalVectorT;
	normalVector(0, 0) = normal[0];
	normalVector(1, 0) = normal[1];
	normalVector(2, 0) = normal[2];
	normalVectorT = normalVector.transpose();
	ColumnVector3 edgeVectorIn(3, 1);
	ColumnVector3 edgeVectorOut(3, 1);

	// identity matrix
	Matrix3x3 identity;
	identity.setIdentity();

	Matrix3x3 M;
	M = M.setZero();

	Matrix3x3 normalMult;
	normalMult = normalVector * normalVectorT;
	Scalar vecLength;

	Scalar sumArea = getVoronoiArea(vertex) * 2;
	// for each outgoing half edges
	for (Mesh::VOHIter outgoing = m_mesh->voh_begin(vertex); outgoing; ++outgoing)
	{
		// retrieve vector of a half edge and put it into matrices
		m_mesh->calc_edge_vector( outgoing, outVector);
			
		edgeVectorIn(0, 0) = -outVector[0];
		edgeVectorIn(1, 0) = -outVector[1];
		edgeVectorIn(2, 0) = -outVector[2];
			
		edgeVectorOut(0, 0) = outVector[0];
		edgeVectorOut(1, 0) = outVector[1];
		edgeVectorOut(2, 0) = outVector[2];

		// compute Tij
				
		Tij = (identity - normalMult) * edgeVectorIn;
		Tij.normalize();
		TijT = Tij.transpose();

		// compute Kij
		vecLength = outVector.length();
		Kij = ((normalVectorT * edgeVectorOut)(0) * 2) / (vecLength * vecLength);

		// compute weigth
		face1 = m_mesh->face_handle(outgoing);
		face2 = m_mesh->opposite_face_handle(outgoing);

		w = 0;
		if (m_mesh->is_valid_handle(face1))
		{
			if (isObtuse(face1, he))
				w += getVoronoiAreaTriO(face1, vertex);
			else w += getVoronoiAreaTri(face1, vertex);
		}
		if (m_mesh->is_valid_handle(face2))
		{
			if (isObtuse(face2, he))
				w += getVoronoiAreaTriO(face2, vertex);
			else w += getVoronoiAreaTri(face2, vertex);
		}
		w /= sumArea;
		// add to M
		M += w * Kij * Tij * TijT;	
	}

	// comp householder matrix (restrict matrix to normal plane only)
	ColumnVector3 E;
	E(0, 0) = 1.0;
	E(1, 0) = 0.0;
	E(2, 0) = 0.0;
		
	ColumnVector3 W;
	RowVector3 WT;
	if ((E - normalVector).norm() > (E + normalVector).norm())
		W = E - normalVector;
	else
		W = E + normalVector;
	
	vecLength = W.norm();
	W /= vecLength;
	WT = W.transpose();

	Matrix3x3 Q = identity - 2 * (W * WT);

	Matrix3x3 result;
	result = Q.transpose() * M * Q;

	Matrix2x2 subM;
	
	// save sub matrix of householder transformed matrix M into 2x2 matrix
	subM(0, 0) = result(1, 1);
	subM(0, 1) = result(1, 2);
	subM(1, 0) = result(2, 1);
	subM(1, 1) = result(2, 2);

	// compute Givens rotation angle (angle necessary to null element (1, 0))
	Scalar r = sqrt(subM(0, 0) * subM(0, 0) + subM(1, 0)*subM(1, 0));
	Scalar c = subM(0, 0)/r;
	Scalar s = subM(1, 0)/r;

	// rotate T1 and T2 vectors and compute min and max curvature
	Normal T1 = Normal(Q(0, 1), Q(1, 1),  Q(2, 1));
	Normal T2 = Normal(Q(0, 2), Q(1, 2),  Q(2, 2));
	//max = c * T1.operator*(c) - s*T2;
	//min = s * T1 + c*T2;
	c = cos(c);
	s = sin(s);
	max = T1.operator*(c) - T2.operator*(s);
	min = T1.operator*(s) + T2.operator*(c);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Calculates minimal curvature and for all mesh vertices
// @return True, if calculated successfully
// @todo Need to implement directions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
bool OMCurvature<Mesh, Scalar>::calcMinCurvature()
{
	if (!m_mesh->IsTriMesh) return false;

	Scalar delta;
	Scalar aux;
	Normal min, max;
	Mesh::VertexIter end = m_mesh->vertices_end();

	// for each vertex of the model
	for (Mesh::VertexIter it = m_mesh->vertices_begin(); it != m_mesh->vertices_end(); ++it)
	{
		// calculate only if it is not boundary
		if (!m_mesh->is_boundary(it))
		{
			aux = getMeanOperator(it).length()/2;
			delta = aux * aux - getGaussOperator(it);
			m_mesh->property(m_curvMagHandle, it) = aux - sqrt(delta);
			getCurvatureDirections(it, max, min);
			min.normalize();
			m_mesh->property(m_curvVectorHandle, it) = min;
		}
		// else return zeros
		else
		{
			m_mesh->property(m_curvMagHandle, it) = 0.0;
			m_mesh->property(m_curvVectorHandle, it) = Normal(0.0, 0.0, 0.0);
		}
	}
	return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Calculates maximal curvature and for all mesh vertices
// @return True, if calculated successfully
// @todo Need to implement directions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
bool OMCurvature<Mesh, Scalar>::calcMaxCurvature()
{
	if (!m_mesh->IsTriMesh) return false;

	Scalar delta;
	Scalar aux;
	Normal min, max;

	Mesh::VertexIter end = m_mesh->vertices_end();
	// for each vertex of the model
	for (Mesh::VertexIter it = m_mesh->vertices_begin(); it != m_mesh->vertices_end(); ++it)
	{
		// calculate only if it is not boundary
		if (!m_mesh->is_boundary(it))
		{
			aux = getMeanOperator(it).length()/2;
			delta = aux * aux - getGaussOperator(it);
			m_mesh->property(m_curvMagHandle, it) = aux + sqrt(delta);
			getCurvatureDirections(it, max, min);
			max.normalize();
			m_mesh->property(m_curvVectorHandle, it) = max;
		}
		// else return zeros
		else
		{
			m_mesh->property(m_curvMagHandle, it) = 0.0;
			m_mesh->property(m_curvVectorHandle, it) = Normal(0.0, 0.0, 0.0);
		}
	}
	return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Calculates Gauss curvature and for all mesh vertices
// @return True, if calculated successfully
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
bool OMCurvature<Mesh, Scalar>::calcGaussCurvature()
{
	if (!m_mesh->IsTriMesh) return false;

	Mesh::VertexIter end = m_mesh->vertices_end();
	// for each vertex of the model
	for (Mesh::VertexIter it = m_mesh->vertices_begin(); it != m_mesh->vertices_end(); ++it)
	{
		// calculate only if it is not boundary
		if (!m_mesh->is_boundary(it))
			m_mesh->property(m_curvMagHandle, it) = getGaussOperator(it);
		// else return zero
		else
			m_mesh->property(m_curvMagHandle, it) = 0.0;

		// Gauss curvature has not a normal
		m_mesh->property(m_curvVectorHandle, it) = Normal(0.0, 0.0, 0.0);
	}
	return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Calculates mean curvature and its direction for all mesh vertices
// @return True, if calculated successfully
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
bool OMCurvature<Mesh, Scalar>::calcMeanCurvature()
{
	if (!m_mesh->IsTriMesh) return false;

	Mesh::VertexIter end = m_mesh->vertices_end();
	// for each vertex of the model
	for (Mesh::VertexIter it = m_mesh->vertices_begin(); it != m_mesh->vertices_end(); ++it)
	{
		// calculate only if it is not boundary
		if (!m_mesh->is_boundary(it))
		{
			Normal vector = getMeanOperator(it);
			m_mesh->property(m_curvMagHandle, it) = vector.length() / 2.0;
			vector.normalize();
			m_mesh->property(m_curvVectorHandle, it) = vector;
		}
		// else return zeros
		else
		{
			m_mesh->property(m_curvMagHandle, it) = 0.0;
			m_mesh->property(m_curvVectorHandle, it) = Normal(0.0, 0.0, 0.0);
		}
	}
	return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Predicate, is triangle obtuse? (Is one of its angles obtuse?)
// @param face Specified triangle
// @param half_edge Half edge handle pointing on found obtuse angle (if not obtuse, it is not specified)
// @return true, if triangle has an obtuse angle
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
bool OMCurvature<Mesh, Scalar>::isObtuse(FaceHandle face, HalfedgeHandle &half_edge)
{
	Mesh::FEIter it;

	for(it = m_mesh->fe_begin(face); it; ++it)
	{
		if (m_mesh->calc_sector_angle(it.current_halfedge_handle()) > M_PIHALF)
		{
			half_edge = it.current_halfedge_handle();
			return true;
		}
	}
	return false;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Computes voronoi area of obtuse triangle from given vertex 
// @param face Given triangle
// @param face Vertex handle
// @return Area of voronoi region
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
Scalar OMCurvature<Mesh, Scalar>::getVoronoiAreaTriO(FaceHandle face, VertexHandle vertex)
{
	Mesh::FHIter it = m_mesh->fh_begin(face);
	m_mesh->fv_begin(face);
	unsigned int i = 0;
	// find a halfedge, which points to a vertex and is part of this face (which is not always true with registered edge)
	for (; it; ++it)
	{
		if (m_mesh->to_vertex_handle(it) == vertex) 
			break;
		++i;
	}
	// if there is a problem (it should't, but.....)
	if (m_mesh->to_vertex_handle(it) != vertex) return 0.0;

	// if there is already computed area
	Scalar result = m_mesh->property(m_areas, face)[i];
	if (result != 0) return result;

	// pre-compute next and previous half edges
	HalfedgeHandle next = ++it;
	HalfedgeHandle prev = ++it;
	++it;

	Scalar angleIt = m_mesh->calc_sector_angle(it);
	Scalar angleNext = m_mesh->calc_sector_angle(next);
		
	/*
	Supose this triangle, we have three possibilities:
		it points to A, an obtuse angle
		it points to B, successor of A
		it points to C, predecessor of A

			C
			\___/__
				\     
				 \
				  \
				  _\
  					\_____________________\__
					A							B

	*/
		
	// we most find these three cases and compute different numbers
	if (angleIt < M_PIHALF)
	{
		// if computing point B
		if (angleNext < M_PIHALF)
		{
			Scalar lenght = m_mesh->calc_edge_length(it);
			Scalar result = (lenght * lenght * tan(angleIt))/8.0;
			m_mesh->property(m_areas, face)[i] = result;
			return result;
		}
		// if computing point C
		else
		{
			Scalar lenght = m_mesh->calc_edge_length(next);
			Scalar result = (lenght * lenght * tan(angleIt))/8.0;
			m_mesh->property(m_areas, face)[i] = result;
			return result;
		}
	}
	// if computing point A
	else 
	{	
		Scalar result = m_mesh->calc_sector_area(it);
		Scalar lenghtB = m_mesh->calc_edge_length(next);
		result -= (lenghtB * lenghtB * tan(angleNext))/8.0;

		Scalar lenghtC = m_mesh->calc_edge_length(it);
		result -= ((lenghtC * lenghtC * tan(m_mesh->calc_sector_angle(prev)))/8.0);
		m_mesh->property(m_areas, face)[i] = result;
		return result;
	}
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//// Computes voronoi area of obtuse triangle from given vertex 
//// @param face Given triangle
//// @param face Vertex handle
//// @return Area of voronoi region
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//template <class Mesh, class Scalar>
//Scalar OMCurvature<Mesh, Scalar>::getVoronoiAreaTriO(VertexHandle *vertices)
//{
//	Mesh::Normal pointing	= vertices[0] - vertices[2];
//	Mesh::Normal next		= vertices[1] - vertices[0];
//	Mesh::Normal prev		= vertices[2] - vertices[1];
//
//	edges[1] = vertices[2] - vertices[1];
//	edges[2] = vertices[0] - vertices[2];
//
//	Scalar angleIt = (pointing | next) / (pointing.norm() * pointing.norm());
//	Scalar angleNext = (next | prev) / (next.norm() * prev.norm())
//		
//	/*
//	Supose this triangle, we have three possibilities:
//		it points to A, an obtuse angle
//		it points to B, successor of A
//		it points to C, predecessor of A
//
//			C
//			\___/__
//				\     
//				 \
//				  \
//				  _\
//  					\_____________________\__
//					A							B
//
//	*/
//		
//	// we most find these three cases and compute different numbers
//	if (angleIt < M_PIHALF)
//	{
//		// if computing point B
//		if (angleNext < M_PIHALF)
//		{
//			Scalar lenght = pointing.norm();
//			Scalar result = (lenght * lenght * tan(angleIt))/8.0;
//			return result;
//		}
//		// if computing point C
//		else
//		{
//			Scalar lenght = next.norm();
//			Scalar result = (lenght * lenght * tan(angleIt))/8.0;
//			return result;
//		}
//	}
//	// if computing point A
//	else 
//	{	
//		Scalar p = (pointing.norm() + next.norm() + prev.norm()) / 2.0;
//		Scalar result = sqrt(p * (p - pointing.norm()) * (p * next.norm()) * (p * prev.norm()));
//		Scalar lenghtB = next.norm();
//		result -= (lenghtB * lenghtB * tan(angleNext))/8.0;
//
//		Scalar lenghtC = pointing.norm();
//		result -= ((lenghtC * lenghtC * tan((pointing | prev) / (pointing.norm() * prev.norm())))/8.0);
//		return result;
//	}
//}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Computes voronoi area of non-obtuse triangle from given vertex 
// @param face Given triangle
// @param face Vertex handle
// @return Area of voronoi region
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
Scalar OMCurvature<Mesh, Scalar>::getVoronoiAreaTri(FaceHandle face, VertexHandle vertex)
{
	Mesh::FHIter it = m_mesh->fh_begin(face);
	unsigned int i = 0;
	// find a halfedge, which points to a vertex and is part of this face (which is not always true with registered edge)
	for (; it; ++it)
	{		
		if (m_mesh->to_vertex_handle(it) == vertex) 
			break;
		++i;
	}
	if (m_mesh->to_vertex_handle(it) != vertex) return 0.0;

	// if there is already computed area
	Scalar result = m_mesh->property(m_areas, face)[i];
	if (result != 0) return result;

	// pre-compute next and previous half edges
	Mesh::FHIter hedge = it;
	HalfedgeHandle next = ++it;
	HalfedgeHandle prev = ++it;
	/*
		We have this situation:
		  computed vertex
				A
			   /\_
			  /  \ 
	  next   /    \ hedge
			/      \
		  |/        \
		  /________\_\
		C			B
		      prev

		Sector angle is angle between hedge and successor
		computing: (1/8) * (lenght(AC) * cot(B) + lenght(AB) * cot(C))
	*/

	Scalar auxLenght = m_mesh->calc_edge_length(hedge);
	Scalar angle = m_mesh->calc_sector_angle(next);
	
	if (angle > 0 && angle < M_PIHALF)
		result =  auxLenght * auxLenght * 1/tan(angle);
		
	angle= m_mesh->calc_sector_angle(prev);
	auxLenght = m_mesh->calc_edge_length(next);
	if (angle > 0 && angle < M_PIHALF)
		result += auxLenght * auxLenght * 1/tan(angle);

	result /= 8.0;
	m_mesh->property(m_areas, face)[i] = result;
	return result;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Computes mean curvature operator (vector, which direction equals mean curvature direction, its norm equals 2*mean curvature) for specified vertex
// @param vertex Vertex handle
// @return mean curvature operator
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Scalar>
typename OMCurvature<Mesh, Scalar>::Normal OMCurvature<Mesh, Scalar>::getMeanOperator(VertexHandle vertex)
{
	HalfedgeHandle opp;
	Normal result(0.0, 0.0, 0.0);

	Normal vector;
	Scalar aux;
	Scalar tang;

	// for each outgoing halfedge
	for (Mesh::VertexOHalfedgeIter it = m_mesh->voh_begin(vertex); it; ++it)
	{
		// compute cotangents
		opp = m_mesh->opposite_halfedge_handle(it);
		tang = tan(m_mesh->calc_sector_angle(m_mesh->next_halfedge_handle(it)));
		if (tang != 0)
			aux = 1/tang;
		tang = tan(m_mesh->calc_sector_angle(m_mesh->next_halfedge_handle(opp)));
		if (tang != 0)
			aux += 1/tang;
			
		m_mesh->calc_edge_vector(opp, vector);
		vector *= aux;
		result += (Normal)vector;
	}

	// and normalize with voronoi area region
	aux = getVoronoiArea(vertex);
	if (aux != 0)
	{
		result /= (2*aux);
		return result;
	}
	else return Normal(0.0, 0.0, 0.0);		
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Computes Gauss curvature value for specified vertex
// @param vertex Vertex handle
// @return Gauss curvature magnitude
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<class Mesh, class Scalar>
Scalar OMCurvature<Mesh, Scalar>::getGaussOperator(VertexHandle vertex)
{
	HalfedgeHandle opp;
	Normal result(0.0, 0.0, 0.0);

	Normal vector;
	Scalar aux = 0.0;
	Scalar area;
		
	// for each outgoing halfedge
	for (Mesh::VertexIHalfedgeIter it = m_mesh->vih_begin(vertex); it; ++it)
		aux += m_mesh->calc_sector_angle(it);

	aux = M_TWOPI - aux;
	area = getVoronoiArea(vertex);
	if (area != 0)
	{
		return aux/area;	
	}
	else return 0;
		
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Computes mean curvature operator (vector, which direction equals mean curvature direction, its norm equals 2*mean curvature) for specified vertex
// @param vertex Vertex handle
// @return mean curvature operator
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<class Mesh, class Scalar>
Scalar OMCurvature<Mesh, Scalar>::getVoronoiArea(VertexHandle vertex)
{
	Mesh::VFIter it, end;
	end = m_mesh->vf_end(vertex);

	HalfedgeHandle hedge;
	Scalar result = 0.0;
	for (it = m_mesh->vf_begin(vertex); it; ++it)
	{
		if (!isObtuse(it, hedge))
		{
			result += getVoronoiAreaTri(it, vertex);
		}
		else 
		{
			result += getVoronoiAreaTriO(it, vertex);
		}
	}
	return result;
}

#endif // _OM_CURVATURE_HXX_