#include "smoothing.h"

#include "OpenMesh/Core/Mesh/SmartHandles.hh"
#include <OpenMesh/Core/Utils/PropertyManager.hh>
#include "util.h"




/*
   	  vα/--\
   		/ 	 ------\   v
   	   /  			---X
   	  /			/---- /	\
   	 /		/---	 /	 -\
   	/  /----		/	   \
 vi	---			   |	 	\
  	  \--		   /	 	 -\
  	     \-		  /		 	/--\
  		   \--	 /	/-------
   	   	      \-+---
               vβ
*/

Point laplace_beltrami(const MyMesh &mesh, VertexHandle vert) {
	Point sum {0, 0, 0};
	qreal area = 0;
	size_t count = 0;
	Point v = mesh.point(vert);
	for (HalfedgeHandle v_vi : mesh.voh_range(vert)) {
		Point vi = mesh.point(mesh.to_vertex_handle(v_vi));

		HalfedgeHandle vi_v = mesh.opposite_halfedge_handle(v_vi);
		HalfedgeHandle v_va = mesh.next_halfedge_handle(vi_v);
		Point va = mesh.point(mesh.to_vertex_handle(v_va));

		HalfedgeHandle vi_vb = mesh.next_halfedge_handle(v_vi);
		Point vb = mesh.point(mesh.to_vertex_handle(vi_vb));

		qreal a = angle_between(vi, va, v);
		qreal b = angle_between(v, vb, vi);
		sum += (1/qTan(a) + 1/qTan(b)) * (vi - v);
		// sum += (vi - v);
		area += triangle_area(v, vi, vb);
		// qDebug("a: %lf, b: %lf, sum: %f %f %f, area: %lf",
		// 	   a, b, sum[0], sum[1], sum[2], area);
		count++;
	}
	area /= 3.;
	// Point tmp(sum / (2*area));
	// qDebug() << tmp[0] << tmp[1] << tmp[2];
	return sum / (2.*area);
	// return sum / count;
}


void smooth(MyMesh &mesh) {
	auto new_pos = OpenMesh::makeTemporaryProperty<VertexHandle, Point>(mesh);
	for (VertexHandle v : mesh.vertices()) {
		if (!mesh.is_boundary(v)) {
			new_pos[v] = mesh.point(v) + laplace_beltrami(mesh, v);
			// mesh.set_point(v, mesh.point(v) + laplace_beltrami(mesh, v));
		}
	}
	for (VertexHandle v : mesh.vertices()) {
		if (!mesh.is_boundary(v)) {
			mesh.set_point(v, new_pos[v]);
		}
	}
}
