//==============================================================================
/*! \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/21                          \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 projections on tangent planes in each of the vertices
 */
#ifndef _OM_PROJECTOR_HXX_
#define _OM_PROJECTOR_HXX_

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Constructor - initializes vital variables
// @param mesh Pointer to a mesh to work with
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Matrix>
OMProjector<Mesh, Matrix>::OMProjector(MeshT *mesh, OpenMesh::VPropHandleT<MatrixT> propertyHandle) : tree(mesh)
{
	m_mesh = mesh;
	m_zDir = ZDIR_Z;
	m_propertyHandle = propertyHandle;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Method computes tangent rasters on all the vertices
// @param length Length of a square raster (real length in mesh space)
// @param resolution Number of pixels on MatrixT edge
// @param zDir Use ZDIR consts for choose which component will be used as Z direction
// @param xdirection Direction of X axis on MatrixT (for ex. Maximum curvature direction)
// @param lengthRelative If true, length is computed as % of median edges lengths, if not, length is static
// @return True, if computation ended successfully, false otherwise
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Matrix>
bool OMProjector<Mesh, Matrix>::ComputeMatrices(ScalarT length, ScalarT resolution, int zDir, bool lengthRelative, int xdirection)
{	
	if (!m_mesh->IsTriMesh) return false;
	
	// init auxiliary variables and check normals
	m_resolution = resolution;
	
	VectorT direction;

	// precomp if projecting curvature as ZDir 
	if (zDir == ZDIR_CURVATURE)
	{
		m_zDir = ZDIR_CURVATURE;
		m_mesh->request_face_normals();
		m_mesh->request_vertex_normals();
		MeshT::FaceIter end = m_mesh->faces_end();
		VectorT normal;
		VectorT firstVec;
		VectorT secondVec;

		for (MeshT::FaceIter face = m_mesh->faces_begin(); face != end; ++face)
		{
			MeshT::FHIter first = m_mesh->fh_begin(face);
			m_mesh->calc_edge_vector(first, firstVec);
			m_mesh->calc_edge_vector(++first, secondVec);
			normal = cross(firstVec, secondVec);
			normal.normalize();
			m_mesh->set_normal(face, normal);
		}
		m_mesh->update_vertex_normals();
	}

	// precomp if computing zdir or normals
	else if (zDir == ZDIR_Z || zDir == ZDIR_NORMAL)
	{
		m_zDir = zDir;
		if (!m_mesh->has_face_colors() || !m_mesh->has_vertex_normals())
		{
			m_mesh->request_face_normals();
			m_mesh->request_vertex_normals();
			m_mesh->update_normals();
		}
	}

	// if length is set as relative, we must compute median and multiply it with length
	std::vector<ScalarT> all;
	MeshT::EdgeIter ende = m_mesh->edges_end();
	
	for (MeshT::EdgeIter edge = m_mesh->edges_begin(); edge != ende; ++edge)
		all.push_back(m_mesh->calc_edge_length(edge));
	
	std::sort(all.begin(), all.end());
	m_length = all[all.size()/2];
	
	if (lengthRelative)
	{
		length = all[all.size()/2] * length;
	}
	
	// pass all vertices and compute tangent matrices
	MeshT::VertexIter end = m_mesh->vertices_end();
	unsigned int i = 0;
	unsigned int total = m_mesh->n_vertices();
	
	/*std::ofstream file;
	file.open(filename,  std::fstream::app);*/

	int N = m_mesh->n_vertices();
	switch (xdirection)
	{
		case XDIR_CURVATURE:
			#pragma omp parallel// shared(m_mesh)
			{
				#pragma omp for schedule(dynamic)
				for (int i = 0; i < N; ++i)
				{
					MeshT::VertexHandle vertex = m_mesh->vertex_handle(i);
					if (i%5000 == 0) MDS_LOG_NOTE("Computed " << i << " vertices of " << total << ".");
					direction = m_mesh->curvature(vertex);
					// create a transformation MatrixT - vertex normal specifies Z direction, curvature direction specifies X direction
					OMTransformationSolver<PointT> solver(length, resolution, m_mesh->normal(vertex), direction, m_mesh->point(vertex));
					rasterizeVertex(solver, vertex);
				}
			}
			break;
		case XDIR_DO_NOT_CHECK:
			#pragma omp parallel //shared(m_mesh)
			{
				#pragma omp for schedule(dynamic)
				for (int i = 0; i < N; ++i)
				//for (MeshT::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
				{
					MeshT::VertexHandle vertex = m_mesh->vertex_handle(i);
					if (i%5000 == 0) MDS_LOG_NOTE("Computed " << i << " vertices of " << total << ".");
					//++i;
					// create a transformation MatrixT - vertex normal specifies Z direction, curvature direction specifies X direction
					OMTransformationSolver<PointT> solver(length, resolution, m_mesh->normal(vertex), PointT(0.0, 0.0, 0.0), m_mesh->point(vertex));
					rasterizeVertex(solver, vertex);
				}
			}
			break;

		default:
			return false;
	}
	return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Function rasterizes one triangle given by face handle on a MatrixT with use of transformation MatrixT
// @param solver OMTransformationSolver with saved transformation MatrixT
// @param face Face handle of a triangle
// @param MatrixT MatrixT on which we will rasterize a face
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Matrix>
void OMProjector<Mesh, Matrix>::rasterizeOnMatrixT(OMTransformationSolver<PointT> &solver, FaceHandleT face, MatrixT &matrix)
{
	PointT vertices[3];
	PointT current;
	// compute face normal for face equation
	VectorT normal = solver.transformTo2DLinear(m_mesh->normal(face));
	normal.normalize_cond();

	// find all tri vertices
	unsigned char i = 0;

	for (MeshT::FVIter it = m_mesh->fv_begin(face); it; ++it, ++i)
	{
		vertices[i] = solver.transformTo2D(m_mesh->point(it));
		//std::cout << vertices[i][0] << " " << vertices[i][1] << " " << vertices[i][2] << std::endl;
	}

	/////////////////////////////////////
	VectorT realNormal= (vertices[1]-vertices[0]) % (vertices[2]-vertices[1]);
	//std::cout << "Normal " << realNormal[0] << " " << realNormal[1] << " " << realNormal[2] << std::endl;
	if (realNormal[2] < 0)
	{
		current = vertices[2];
		vertices[2] = vertices[0];
		vertices[0] = current;
	}
	/////////////////////////////////////
	if (m_zDir == ZDIR_CURVATURE)
	{
		int i = 0;
		for (MeshT::FVIter it = m_mesh->fv_begin(face); it; ++it, ++i)
		{
			vertices[i][2] = m_mesh->curvatureMagnitude(it);
			++i;
		}
		normal = (vertices[1] - vertices[0]) % (vertices[2] - vertices[0]);
		normal.normalize();
	}

	ScalarT equationFract = normal[2] * solver.getPixelSize();
	if (equationFract == 0.0) equationFract = 1.0;
	// compute D coefficient of face equation (N1 * x + N2 * y + N3 * z + d = 0)
	ScalarT d = -(normal[0]*vertices[0][0] + normal[1]*vertices[0][1] + normal[2]*vertices[0][2]);

	// Use the triangle iterator and rasterize this triangle
	OMToolkit::OMTriIterator<PointT> triangle(vertices);

	

	if (m_zDir == ZDIR_NORMAL)
	{
		VectorT Z(0.0, 0.0, 1.0);
		while(!triangle.isEnd())
		{
			current[0] = triangle.getX();
			current[1] = triangle.getY();
			
			if (current[0] >= m_MatrixTDelim.minimum[0] && current[0] <= m_MatrixTDelim.maximum[0] &&	
				current[1] >= m_MatrixTDelim.minimum[1] && current[1] <= m_MatrixTDelim.maximum[1])// && matrix(current[1], current[0]) > (sin(acos(normal | Z))))
					matrix(current[1], current[0]) = (sin(acos(normal | Z)));
			++triangle;
		}
	}
	else
		while(!triangle.isEnd())
		{
			current[0] = triangle.getX();
			current[1] = triangle.getY();
			//std::cout << current[0] << " " << current[1] << std::endl;  
			if (current[0] >= m_MatrixTDelim.minimum[0] && current[0] <= m_MatrixTDelim.maximum[0] &&	
				current[1] >= m_MatrixTDelim.minimum[1] && current[1] <= m_MatrixTDelim.maximum[1])// && abs(matrix(current[1], current[0])) > abs((normal[0] * current[0] + normal[1] * current[1] + d)/equationFract))
					matrix(current[1], current[0]) = (normal[0] * current[0] + normal[1] * current[1] + d)/equationFract;
			//MDS_LOG_NOTE((normal[0] * current[0] + normal[1] * current[1] + d)/equationFract);
			//MDS_LOG_NOTE(equationFract);
			++triangle;
		}
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Function rasterizes neighbourhood of a vertex into a raster given by transformation solver
// Result is saved in MatrixT property of a vertex
// @param solver Transformation solver with saved transformation MatrixT
// @param vertex Vertex on which we compute a tangent raster
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class Matrix>
void OMProjector<Mesh, Matrix>::rasterizeVertex(OMTransformationSolver<PointT> &solver, VertexHandleT vertex)
{
	int INITIAL;
	
	if (m_zDir == ZDIR_Z)
		INITIAL = m_length;
	else
		INITIAL = M_PI;

	std::set<int> addedFaces;
	std::set<int> searchedVertices;
	VertexHandleT originalHandle = vertex;
	PointT currentPoint;
	PointT transformed;
	int index;

	// create a new instance of MatrixT to be computed
	MatrixT matrix(m_resolution, m_resolution);
	matrix.setConstant(INITIAL);

	m_MatrixTDelim.minimum = PointT(0.0, 0.0, 0.0);
	m_MatrixTDelim.maximum = solver.getMaxBounds();
	
	// stack to simulate recursive seed growth
	std::vector<VertexHandleT> stack;
	stack.push_back(vertex);	

	std::vector<FaceHandleT> facesToRasterize;
	//// go recursive to neighbours
	while (!stack.empty())
	{
		vertex = stack.back();
		stack.pop_back();

		currentPoint = m_mesh->point(vertex);
			
		// rasterize all faces around current vertex
		for (MeshT::VFIter around = m_mesh->vf_begin(vertex); around; ++around)
		{
			// get face index and test, if it wasn't rasterized yet
			index = around.handle().idx();
			if (addedFaces.count(index) == 0)
			{
				// if not, save its index and rasterize it
				addedFaces.insert(index);
				facesToRasterize.push_back(around);
				//rasterizeOnMatrixT(solver, around, matrix);
			}
		}
		// then look for all neighbouring vertices and test, if they fit in tangent raster
		for (MeshT::VVIter around = m_mesh->vv_begin(vertex); around; ++around)
		{
			index = around.handle().idx();

			// if it was not tested yet...
			if (searchedVertices.count(index) == 0)
			{
				// transform it into raster coordinates
				transformed = solver.transformTo2D(m_mesh->point(around));

				// mark as searched
				searchedVertices.insert(index);

				// if it is in raster, save it for future search
				if (transformed[0] >= m_MatrixTDelim.minimum[0] && transformed[0] <= m_MatrixTDelim.maximum[0] &&	
					transformed[1] >= m_MatrixTDelim.minimum[1] && transformed[1] <= m_MatrixTDelim.maximum[1])
				{
					stack.push_back(around);
					for (MeshT::VVIter around2 = m_mesh->vv_begin(around); around2; ++around2)
					{
						index = around2.handle().idx();
						if (searchedVertices.count(index) == 0)
						{
							stack.push_back(around2);
							searchedVertices.insert(index);
						}
					}
				}
					
			}
		}
	}
	unsigned int total = facesToRasterize.size();

	// add all tris sharing boundary edges
	for (unsigned int i = 0; i < total; ++i)
	{
		for (MeshT::FFIter around = m_mesh->ff_begin(facesToRasterize[i]); around; ++around)
			facesToRasterize.push_back(around);
	}

	total = facesToRasterize.size();
	for (unsigned int i = 0; i < total; ++i)
		rasterizeOnMatrixT(solver, facesToRasterize[i], matrix);

	// Repair holes (find by raycasting)
	Mesh::Normal normal = m_mesh->normal(originalHandle);
	normal.normalize_cond();
	
	Mesh::FaceHandle face;
	
	for(int x = 0; x < m_resolution; ++x)
		for(int y = 0; y < m_resolution; ++y)
		{
			if (matrix(y, x) == INITIAL)
			{
				Mesh::Point point(x, y, 0.0);
				point = solver.transformToMesh(point);
				
				tree.getPassingFace(point, normal, face);

				if (face.is_valid())
					rasterizeOnMatrixT(solver, face, matrix);
			}
		}

	m_mesh->property(m_propertyHandle, originalHandle) = matrix;
}

#endif _OM_PROJECTOR_HXX_