//==============================================================================
/*! \file
 * OpenMesh Toolkit for mesh analysis    \n
 * Copyright (c) 2010 by Rostislav Hulik     \n
 *
 * Author:  Rostislav Hulik, rosta.hulik@gmail.com  \n
 * Date:    2010/10/20                          \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 for descriptor computation in mesh data
 */
 
#include <OMToolkit/OMMatrixDescriptors.h>
#include <OMToolkit\OMTransformationSolver.h>
#include <OMToolkit\OMProjector.h>
#include <OMToolkit\OMCurvature.h>
#include <OMToolkit\OMMatrixCurvature.h>
#include <MDSTk\Math\mdsMatrix.h>
#include <Eigen\Core>
#include <Eigen\LU>
#include <Eigen\QR>
#include <opencv2\opencv.hpp>
#include <opencv2\core\core.hpp>
#include <opencv2\imgproc\imgproc_c.h>

using namespace cv;

namespace OMToolkit	{

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// Constructor initializes this class for ongoing computations
	// @param mesh Input mesh
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	OMMatrixDescriptors::OMMatrixDescriptors(MeshT *mesh)
	{
		m_mesh = mesh;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// The structure for magnitude-vertex handle pair
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	typedef struct 
	{
		float magnitude;
		Types::ModuleMeshd::VertexIter handle;
	} TSorter;

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// The comparer for TSorter struct
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	struct compareByMag{
		bool operator()(const TSorter &a, const TSorter &b)
		{
			return (a.magnitude > b.magnitude);
		}
	}; 

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// The function computes feature points using pre-computed curvature on the mesh
	// @param matrixH The matrix handle
	// @param curvatureMagH The curvature magnitude handle
	// @param finalNum The final number of feature points in the mesh
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	bool OMMatrixDescriptors::ComputePoints(OpenMesh::VPropHandleT<MatrixT> matrixH, OpenMesh::VPropHandleT<AScalarT> curvatureMagH, int finalNum)
	{
		MatrixT matrix;

		MeshT::VertexIter end = m_mesh->vertices_end();
		std::vector<TSorter> values;

		// Pass all vertices and index their curvature magnitude
		for (MeshT::VertexIter vertex = m_mesh->vertices_begin() ; vertex != end; ++vertex)
		{
			TSorter aux;
			aux.magnitude = m_mesh->property(curvatureMagH, vertex);
			aux.handle = vertex;
			values.push_back(aux);

			m_mesh->property(curvatureMagH, vertex) = 0.0;
		}

		// Sort curvatures
		std::sort(values.begin(), values.end(), compareByMag());

		// Get finalNum of the greatest
		for (int i = 0; i < (int)values.size() && i < finalNum; ++i)
		{
			m_mesh->property(curvatureMagH, values[i].handle) = 255.0;
		}
		

	return false;
	} 

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// The function extracts the descriptors from the points marked as feature points
	// @see ComputePoints
	// @param matrixH The matrix handle
	// @param curvatureMagH The curvature magnitude handle
	// @param matrix_size The size of matrix used for descriptor computation (relative to the median of edge lengths)
	// @param matrix_resolution The resolution of the matrix
	// @param desc_diameter The descriptor diameter
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	std::vector<std::vector<float>> OMMatrixDescriptors::ComputeDescriptors(OpenMesh::VPropHandleT<MatrixT> matrixH, OpenMesh::VPropHandleT<AScalarT> curvatureMagH, float matrix_size, int matrix_resolution, float desc_diameter)
	{
		MatrixT matrix;
		MatrixT oldmatrix;
		float curvMag;

		MeshT::VertexIter end = m_mesh->vertices_end();

		std::vector<TSorter> values;

		// Compute lengths for relative size of matrices
		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());
		float m_length = all[all.size()/2];

		// Compute necessary info in mesh
		m_mesh->request_face_normals();
		m_mesh->request_vertex_normals();
		m_mesh->update_normals();


		// Initialize projector and compute the number of feature points
		OMProjector<MeshT, MatrixT> projector(m_mesh, m_mesh->getMatrixHandle());

		int count = 0;
		for (MeshT::VertexIter vertex = m_mesh->vertices_begin() ; vertex != end; ++vertex)
		{
			curvMag = m_mesh->property(curvatureMagH, vertex);
			if (curvMag > 0) count ++;
		}
		
		std::vector<std::vector<float>> output(count);

		// Pass all feature points and compute SIFTs
		count = 0;
		for (MeshT::VertexIter vertex = m_mesh->vertices_begin() ; vertex != end; ++vertex)
		{
			curvMag = m_mesh->property(curvatureMagH, vertex);

			if (curvMag > 0)
			{
				oldmatrix = m_mesh->property(matrixH, vertex);

				// Compute the matrix for SIFT computation (larger size maybe)
				OMTransformationSolver<MeshT::Point> solver(m_length*matrix_size, matrix_resolution, m_mesh->normal(vertex), MeshT::Point(0.0, 0.0, 0.0), m_mesh->point(vertex));
				projector.m_length = m_length;
				projector.m_resolution = matrix_resolution;
				projector.rasterizeVertex(solver, vertex);
				
				// Get the matrix and normalize it
				matrix = m_mesh->property(matrixH, vertex);
				cv::Mat mask(matrix.rows(), matrix.cols(), CV_8UC1);
				mask.setTo(255);

				cv::Mat matrix_cv(matrix.rows(), matrix.cols(), CV_32FC1);

				for (unsigned int i = 0; i < projector.m_resolution; ++i)
				for (unsigned int j = 0; j < projector.m_resolution; ++j)
				{
					matrix_cv.at<float>(i, j) = matrix(i, j);
				}

				double min, max;
				cv::minMaxLoc(matrix_cv, &min, &max);

				cv::Mat matrix_8cv(matrix.rows(), matrix.cols(), CV_8UC1);
				for (unsigned int i = 0; i < projector.m_resolution; ++i)
				for (unsigned int j = 0; j < projector.m_resolution; ++j)
				{
					matrix_8cv.at<unsigned char>(i, j) = (unsigned char)(255.0 / ((matrix_cv.at<float>(i, j) - min) / (max - min)) );
				}

				// Initialize the SIFT extractor and compute the value for the center of the matrix
				cv::Mat descriptors;
				std::vector<cv::KeyPoint> keyPoints;
				keyPoints.push_back(cv::KeyPoint(matrix.cols()/2.0, matrix.rows()/2.0, desc_diameter, 0.0));
				cv::SiftDescriptorExtractor det(1.0, true, false);
				det.compute(matrix_8cv, keyPoints, descriptors);
				
				// Save the values into the output vector
				if (descriptors.rows > 0)
				{
					output[count].push_back(vertex->idx());
					for (int i = 0; i < descriptors.cols; ++i)
					{
						
						output[count].push_back(descriptors.at<float>(0, i));
					}
				}
				
				++count;
				m_mesh->property(matrixH, vertex) = oldmatrix;
			}
		}
	return output;
	} 
} // namespace