#include "smoothing.h"

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




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

Point laplace_beltrami(const MyMesh &mesh, VertexHandle vert) {
	Point sum {0, 0, 0};
	qreal area = 0;
	Point p = mesh.point(vert);
	qreal count = 0;
	for (HalfedgeHandle v_vi : mesh.voh_range(vert)) {
		Point pi = 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 pa = mesh.point(mesh.to_vertex_handle(v_va));
		HalfedgeHandle vi_vb = mesh.next_halfedge_handle(v_vi);
		Point pb = mesh.point(mesh.to_vertex_handle(vi_vb));
		qreal a = angle_between(pi, pa, p);
		qreal b = angle_between(pi, pb, p);
		sum += (cotan(a) + cotan(b)) * (p - pi);
		area += triangle_area(p, pi, pb);
		count++;
	}
	area /= 3.;
	return sum / (2.*count);
}


Eigen::SparseMatrix<qreal> laplacian_matrix(const MyMesh &mesh) {
	size_t n_verts = mesh.n_vertices();
	Eigen::SparseMatrix<qreal> mass(n_verts, n_verts);
	Eigen::SparseMatrix<qreal> cotangent(n_verts, n_verts);
	for (VertexHandle vert : mesh.vertices()) {
		if (mesh.is_boundary(vert)) continue;
		qreal sum = 0;
		qreal area = 0;
		qreal count = 0;
		Point p = mesh.point(vert);
		for (HalfedgeHandle v_vi : mesh.voh_range(vert)) {
			VertexHandle vi = mesh.to_vertex_handle(v_vi);
			Point pi = mesh.point(vi);
			HalfedgeHandle vi_v = mesh.opposite_halfedge_handle(v_vi);
			HalfedgeHandle v_va = mesh.next_halfedge_handle(vi_v);
			Point pa = mesh.point(mesh.to_vertex_handle(v_va));
			HalfedgeHandle vi_vb = mesh.next_halfedge_handle(v_vi);
			Point pb = mesh.point(mesh.to_vertex_handle(vi_vb));
			qreal a = angle_between(pi, pa, p);
			qreal b = angle_between(pi, pb, p);
			qreal w = -(cotan(a) + cotan(b)) / 2.;
			sum += w;
			cotangent.insert(vert.idx(), vi.idx()) = w;
			area += triangle_area(p, pi, pb);
			count++;
		}
		area /= 3.;
		cotangent.insert(vert.idx(), vert.idx()) = -sum;
		mass.insert(vert.idx(), vert.idx()) = 1. / area;
	}
	return mass * cotangent;
}


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);
	// 	}
	// }
	// for (VertexHandle v : mesh.vertices()) {
	// 	if (!mesh.is_boundary(v)) {
	// 		mesh.set_point(v, new_pos[v]);
	// 	}
	// }

	// Approche matricielle
	Eigen::SparseMatrix<qreal> laplacian = laplacian_matrix(mesh);
	size_t n_verts = mesh.n_vertices();
	Eigen::VectorX<qreal> X(n_verts), Y(n_verts), Z(n_verts);
	for (VertexHandle vert : mesh.vertices()) {
		if (mesh.is_boundary(vert)) continue;
		size_t id = vert.idx();
		Point p = mesh.point(vert);
		X(id) = p[0];
		Y(id) = p[1];
		Z(id) = p[2];
	}
	X = laplacian * X;
	Y = laplacian * Y;
	Z = laplacian * Z;
	for (VertexHandle vert : mesh.vertices()) {
		if (mesh.is_boundary(vert)) continue;
		size_t id = vert.idx();
		Point offset {X(id), Y(id), Z(id)};
		mesh.set_point(vert, mesh.point(vert) - offset);
	}
}
