//==============================================================================
/*! \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 filtering informations saved in vertex projection matrix
 */

#ifndef _OM_FILTER_HXX_
#define _OM_FILTER_HXX_

#include <OMToolkit\OMTransformationSolver.h>
///////////////////////////////////////////////////////////////////////////////////////////
// Constructor - initializes vital variables and makes a liaison with target mesh
// @param mesh Mesh to work with
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
OMFilter<Mesh, MatrixT>::OMFilter(Mesh *mesh)
{
	m_mesh = mesh;
	m_loaded = false;
	m_numKernels = 0;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Saves maximal value of attached vertex matrix into attribute vector 
// Save is done by push back
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::MaxAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	Mesh::VertexIter end = m_mesh->vertices_end();
	for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
		m_mesh->getAttributes(vertex).push_back(m_mesh->property(matrixHandle, vertex).maxCoeff());
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Saves median value of attached vertex matrix into attribute vector 
// Save is done by push back
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::MedianAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	Mesh::VertexIter end = m_mesh->vertices_end();
	MatrixT current;
	Scalar *data;
	Scalar *dataCopy;
	int size;
	for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
	{
		current = m_mesh->property(matrixHandle, vertex);
		data = current.data();
		size = current.size();
		dataCopy = new Scalar[size];
		memcpy(dataCopy, data, size * sizeof(Scalar));
			
		std::sort(&(dataCopy[0]), &(dataCopy[size]));
		m_mesh->getAttributes(vertex).push_back(dataCopy[size/2]);
		delete dataCopy;
	}
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Saves mean value of attached vertex matrix into attribute vector 
// Save is done by push back
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::MeanAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	Mesh::VertexIter end = m_mesh->vertices_end();
	MatrixT current;
	Scalar *data;
	int size;
	for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
	{
		current = m_mesh->property(matrixHandle, vertex);
		data = current.data();
		size = current.size();
		Scalar sum = 0.0;
		for (int i = 0; i < size; ++i)
			sum += data[i];
		m_mesh->getAttributes(vertex).push_back(sum /= (Scalar)size);
	}
	return true;
}


///////////////////////////////////////////////////////////////////////////////////////////
// Saves a variance of attached vertex matrix from a plane into attribute vector 
// Save is done by push back
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::VarianceAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	Mesh::VertexIter end = m_mesh->vertices_end();
	MatrixT current;
	Scalar *data;
	int size;
	for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
	{
		current = m_mesh->property(matrixHandle, vertex);
		data = current.data();
		size = current.size();
		Scalar sum = 0.0;
		Scalar variance = 0.0;
		for (int i = 0; i < size; ++i)
			sum += data[i];
		
		sum /= (Scalar)size;
		for (int i = 0; i < size; ++i)
			variance += data[i] - sum;

		m_mesh->getAttributes(vertex).push_back(variance /= (Scalar)size);
	}
	return true;
}
///////////////////////////////////////////////////////////////////////////////////////////
// Saves minimal value of attached vertex matrix into attribute vector 
// Save is done by push back
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::MinAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	Mesh::VertexIter end = m_mesh->vertices_end();
	for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
		m_mesh->getAttributes(vertex).push_back(m_mesh->property(matrixHandle, vertex).minCoeff());
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Saves a max distance of attached vertex matrix from a plane into attribute vector 
// Save is done by push back
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::AbsAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	Mesh::VertexIter end = m_mesh->vertices_end();
	Scalar min, max;
	for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
	{
		min = abs(m_mesh->property(matrixHandle, vertex).minCoeff());
		max = abs(m_mesh->property(matrixHandle, vertex).maxCoeff());
		m_mesh->getAttributes(vertex).push_back(min > max ? min : max);
	}
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Filters matrices attached to each vertex and returns value in the middle
// Provides only fast filtering - computes only value in the middle of a matrix
// Vertex matrix must have the same size asi filter kernel
// Kernel must be loaded from file before calling this function
// Result is saved into attribute vector (push back)
// If there is multiple filtering kernels, result is computed as eucleidian norm of result vector
// (result = sqrt(x^2 + y^2 + ... + z^2))
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::FilterAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	if (m_loaded)
	{
		Mesh::VertexIter end = m_mesh->vertices_end();
		MatrixT current;
		int rows = m_filterKernel[0].rows();
		int cols = m_filterKernel[0].cols();
		for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
		{
			current = m_mesh->property(matrixHandle, vertex);
			if (rows < current.rows() || cols < current.cols()) 
				filterOneSmall(vertex.handle(), matrixHandle);
			else
				m_mesh->getAttributes(vertex).push_back(filterOne(vertex.handle(), matrixHandle));
		}
		return true;
	}
	else
		return false;

}

///////////////////////////////////////////////////////////////////////////////////////////
// Multiplies each element of the matrix with corresponding weight in filter matrix
// Vertex matrix must have the same size asi filter kernel
// Kernel must be loaded from file before calling this function
// If there is multiple filtering kernels, weighting is computed in series
// @param matrixHandle Handle to a matrix - universal property handle
// @return True if all computation completed successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::WeightAll(const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	if (m_loaded)
	{
		Mesh::VertexIter end = m_mesh->vertices_end();
		MatrixT current;
		int rows = m_filterKernel[0].rows();
		int cols = m_filterKernel[0].cols();
		for (Mesh::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
		{
			current = m_mesh->property(matrixHandle, vertex);
			if (rows != current.rows() || cols != current.cols()) return false;

			weightOne(vertex.handle(), matrixHandle);
		}
		return true;
	}
	else
		return false;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Helper function which computes fast filtering of one matrix (used in FilterAll)
// @param vertex Vertex on which we are doing computations
// @param matrixHandle Handle to a matrix - universal property handle
// @return value in the middle of matrix
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
typename OMFilter<Mesh, MatrixT>::Scalar OMFilter<Mesh, MatrixT>::filterOne(typename Mesh::VertexHandle &vertex, const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	// load matrix data
	Scalar *first = m_mesh->property(matrixHandle, vertex).data();
	Scalar *filter = NULL;
	Scalar result = 0.0;

	for (unsigned int i = 0; i < m_numKernels; ++i)
	{
		Scalar aux = 0.0;
		filter = m_filterKernel[i].data();
		int i_max = m_filterKernel[i].size();
		for (int j = 0; j < i_max; ++j) aux += filter[j] * first[j];
		result += aux * aux;
	}
	return sqrt(result);
}

///////////////////////////////////////////////////////////////////////////////////////////
// Helper function which computes fast filtering of one matrix (smaller filter kernel than tangent matrix)
// @param vertex Vertex on which we are doing computations
// @param matrixHandle Handle to a matrix - universal property handle
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
void OMFilter<Mesh, MatrixT>::filterOneSmall(typename Mesh::VertexHandle &vertex, const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	MatrixT matrix = m_mesh->property(matrixHandle, vertex);
	int rows = matrix.rows();
	MatrixT aux(rows, rows);
	
	MatrixT filter = m_filterKernel[0];
	int rowsF = filter.rows();

	int half = rowsF/2;

	// convolution
	for (int r = 0; r < rows; ++r)
		for (int c = 0; c < rows; ++c)
		{
			Scalar result = 0.0;
			for (int rF = -half; rF <= half; ++rF)
				for (int cF = -half; cF <= half; ++cF)
				{
					if (r+rF < 0 || c+cF < 0 || r+rF >= rows || r+rF >= rows) continue;
					else
					{
						result += matrix(r+rF, c+cF) * filter(rF+half, cF+half);
					}
				}
			aux(r, c) = result;
		}

	m_mesh->property(matrixHandle, vertex) = aux;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Helper function which multiplies each matrix element with corresponding weight saved in filter matrix
// @param vertex Vertex on which we are doing computations
// @param matrixHandle Handle to a matrix - universal property handle
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
void OMFilter<Mesh, MatrixT>::weightOne(typename Mesh::VertexHandle &vertex, const OpenMesh::VPropHandleT<MatrixT> &matrixHandle)
{
	// load matrix data
	Scalar *first = m_mesh->property(matrixHandle, vertex).data();
	Scalar *filter = NULL;
	for (unsigned int i = 0; i < m_numKernels; ++i)
	{
		filter = m_filterKernel[i].data();
		int i_max = m_filterKernel[i].size();
		for (int i = 0; i < i_max; ++i) first[i] = filter[i] * first[i];
	}
}

///////////////////////////////////////////////////////////////////////////////////////////
// Function loads a file with saved filter matrix
// File can contain multiple matrices - all are loaded
// Structure of a file is following:
//
// Matrix 2 2
// 3 4
// 1 2
//
// First row contains keyword Matrix with specified dimensions, the other rows contains values
// After this, another header can follow with second matrix
// @param filename File name 
// @return True, if loaded successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::LoadFile(std::string filename)
{
	std::ifstream file;
	file.open(filename);

	// check if file is open
	if (!file.is_open()) return false;
	m_filterKernel.clear();
	m_numKernels = 0;

	// while not eof, we read
	while (!file.eof())
	{
		std::string header;
		file >> header;

		// check header
		if (header != "Matrix") return false;
		int rows, cols;
		file >> rows;
		file >> cols;

		// check rows and cols information
		if (rows <= 0 || cols <= 0) return false;

		m_filterKernel.push_back(MatrixT(rows, cols));

		// read matrix
		for (int r = 0; r < rows; ++r)
			for (int c = 0; c < rows; ++c)
			{
				if (file.eof()) return false;
				file >> m_filterKernel.back()(r, c);
			}
		++m_numKernels;
		// if there is not end of file, continue with another kernel
	}
	file.close();
	m_loaded = true;
	if (m_filterKernel.size() > 0)
	{
		int rows = m_filterKernel[0].rows();
		int cols = m_filterKernel[0].cols();
		for (unsigned int i = 0; i < m_filterKernel.size(); ++i)
		{
			if (rows != m_filterKernel[i].rows() || cols != m_filterKernel[i].cols()) return false;
		}
	}
	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////
// Function saves internal matrices into file
// @param filename File name
// @return True, if saved successfully
///////////////////////////////////////////////////////////////////////////////////////////
template <class Mesh, class MatrixT>
bool OMFilter<Mesh, MatrixT>::SaveFile(std::string filename)
{
	std::ofstream file;
	file.open(filename);

	// check if file is open
	if (!file.is_open() || !m_loaded) return false;

	// write all kernels
	for (unsigned int i = 0; i < m_numKernels; ++i)
	{
		// header + dimensions
		file << "Matrix ";
		file << m_filterKernel[i].rows() << " "<< m_filterKernel[i].cols() << std::endl;

		// each cell
		for (int r = 0; r < m_filterKernel[i].rows(); ++r)
		{
			for (int c = 0; c < m_filterKernel[i].cols(); ++c)
				file << m_filterKernel[i](r, c) << " ";
			file << std::endl;
		}
		file << std::endl;
	}
	// close file
	file.close();
	return true;
}

#endif // _OM_FILTER_HXX_