//==============================================================================
/*! \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 smoothes the mesh using the Local Projections method
 */
 
#include <OMToolkit\OMSmoother.h>
#include <MDSTk\Module\mdsModule.h>

namespace OMToolkit {

#define ZERO 0.0001
	OMSmoother::OMSmoother(MeshT *mesh, OpenMesh::VPropHandleT<NormalT> &curvVectorHandle, OpenMesh::VPropHandleT<ScalarT> &curvHandle, ScalarT gaussF)
{
	m_mesh = mesh;
	m_mesh->add_property(original_positions, "<oPosition>");
	m_mesh->add_property(original_normals, "<oNormal>");
	m_mesh->add_property(weight_direction, "<wDir>");
	m_mesh->add_property(weight, "<wDir>");

	m_mesh->request_face_normals();
	m_mesh->request_vertex_normals();

	m_mesh->update_normals();

	MeshT::VertexIter v_end = m_mesh->vertices_end();
	NormalT aux;
	for (MeshT::VertexIter vertex = m_mesh->vertices_begin(); vertex != v_end; ++vertex)
	{
		m_mesh->property(original_positions, vertex) = m_mesh->point(vertex);
		aux = m_mesh->normal(vertex);
		m_mesh->property(original_normals, vertex) = aux;

		//MDS_LOG_NOTE(m_mesh->property(curvVectorHandle, vertex)[0] << " " << 
		//	m_mesh->property(curvVectorHandle, vertex)[1] << " " << m_mesh->property(curvVectorHandle, vertex)[2]);
		aux = aux % m_mesh->property(curvVectorHandle, vertex);
		
		aux.normalize_cond();
		m_mesh->property(weight_direction, vertex) = aux * m_mesh->property(curvHandle, vertex);
	}

	std::vector<ScalarT> all;
	MeshT::EdgeIter e_end = m_mesh->edges_end();
	for (MeshT::EdgeIter edge = m_mesh->edges_begin(); edge != e_end; ++edge)
		all.push_back(m_mesh->calc_edge_length(edge));
	std::sort(all.begin(), all.end());
	gaussFunc = new OMGaussFunction<ScalarT>(0.0, gaussF * (all.size()/2));
}

void OMSmoother::computeWeightsAround(VertexH vertex)
{
	NormalT dir;
	NormalT normalizedEdge;
	ScalarT aux, aux2;
	ScalarT num = 0.0;
	ScalarT sum = 0.0;
	for (MeshT::VOHIter around = m_mesh->voh_begin(vertex); around; ++around)
	{
		dir = m_mesh->property(original_positions, m_mesh->to_vertex_handle(around)) - m_mesh->property(original_positions, vertex);
		dir.normalize_cond();

		MeshT::VertexHandle handle = m_mesh->to_vertex_handle(around);

		normalizedEdge = m_mesh->property(weight_direction, handle);
		
		aux = normalizedEdge.norm();
		
		//MDS_LOG_NOTE(normalizedEdge[0] << " " << normalizedEdge[1] << " " << normalizedEdge[2]);

		normalizedEdge.normalize_cond();

		aux2 = (dir | normalizedEdge);
		aux2 *= aux2;
		aux2 *= aux;
		m_mesh->property(weight, handle) = aux2;
		sum += aux2;
		num += 1.0f;
	}

	if (aux < ZERO)
	{
		num = 1;///num;
		for (MeshT::VVIter around = m_mesh->vv_begin(vertex); around; ++around)
		{
			m_mesh->property(weight, around) = num;
		}
	}
	else
	{
		sum = num/sum;
		for (MeshT::VVIter around = m_mesh->vv_begin(vertex); around; ++around)
		{
			aux = m_mesh->property(weight, around);
			aux *= sum;
			m_mesh->property(weight, around) = aux;
		}
	}
}


void OMSmoother::smooth(int n, ScalarT alpha)
{
	OpenMesh::VPropHandleT<float> weight2;
	m_mesh->add_property(weight2);

	for (MeshT::VertexIter vertex = m_mesh->vertices_begin(); vertex != m_mesh->vertices_end(); ++vertex)
	{
		m_mesh->property(weight2, vertex) = 0;
	}

	for (MeshT::EdgeIter edge = m_mesh->edges_begin(); edge != m_mesh->edges_end(); ++edge)
	{
		MeshT::HalfedgeHandle heh0 = m_mesh->halfedge_handle(edge, 0);
		MeshT::HalfedgeHandle heh1 = m_mesh->halfedge_handle(edge, 1);
		m_mesh->property(weight2, m_mesh->to_vertex_handle(heh0)) += 1.0;
		m_mesh->property(weight2, m_mesh->to_vertex_handle(heh1)) += 1.0;
	}

	for (int i = 0; i < n; ++i)
	{
		//if (i%2 == 0)
		//	alpha = 0.3;
		//else alpha = -0.301;
		MeshT::VertexIter end = m_mesh->vertices_end();
		m_mesh->update_normals();
		for (MeshT::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
		{
			m_mesh->property(original_positions, vertex) = m_mesh->point(vertex);
		}

		for (MeshT::VertexIter vertex = m_mesh->vertices_begin(); vertex != end; ++vertex)
		{
			computeWeightsAround(vertex);
			PointT point(0.0, 0.0, 0.0);
			PointT point2 = m_mesh->property(original_positions, vertex);
			ScalarT num = 0.0;
			for (MeshT::VVIter around = m_mesh->vv_begin(vertex); around; ++around)
			{
				PointT point3 = m_mesh->property(original_positions, around) - point2;
				//if (i%2 == 0)
				point += gaussFunc->getVal(point3.norm()) * m_mesh->property(weight, around) * m_mesh->property(original_positions, around);// * m_mesh->property(weight2, vertex);
				//else
					//point += gaussFunc->getVal(point3.norm()) /** m_mesh->property(weight, around) **/ * m_mesh->property(original_positions, around);// * m_mesh->property(weight2, vertex);
				num += 1;//m_mesh->property(weight2, vertex);
			}
			
			//point = 0.1f * point;
			point /= num;
			point = point - point2;
			//point *= m_mesh->curvatureMagnitude(vertex);
			point *= alpha;
			point2 = point2 + point;

			m_mesh->set_point(vertex, /*m_mesh->property(original_positions, vertex) + */point2);
		}
	}
	
}

OMSmoother::~OMSmoother()
{
	m_mesh->remove_property(original_positions);
	m_mesh->remove_property(original_normals);
}

}