#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α/--\
        /    ------\   vi
       /            ---X
      /         /---- / \
     /      /---     /   -\
    /  /----        /      \
 vj ---            |        \
      \--          /         -\
         \-       /         /--\
           \--   /  /-------
              \-+---
               vβ
*/


/* Poids pour le laplacien uniforme, constant à 1. */
double uniform_weight(MyMesh &mesh, HalfedgeHandle vi_vj) {
	(void) mesh;
	(void) vi_vj;
	return 1;
}


/* Masse pour le laplacien uniforme, 1 / taille(N1(vi)). */
double uniform_mass(MyMesh &mesh, VertexHandle vi) {
	double count = 0;
	for (VertexHandle v : mesh.vv_range(vi)) {
		(void) v;
		count++;
	}
	return 1. / count;
}


/* Calcule le poids cotangent pour l'arête reliant vi à vj. */
double cotangent_weight(MyMesh &mesh, HalfedgeHandle vi_vj) {
	// Voir le graphique en haut de ce fichier.
	Point pj = mesh.point(mesh.to_vertex_handle(vi_vj));
	HalfedgeHandle vj_vb = mesh.next_halfedge_handle(vi_vj);
	Point pb = mesh.point(mesh.to_vertex_handle(vj_vb));
	HalfedgeHandle vj_vi = mesh.opposite_halfedge_handle(vi_vj);
	Point pi = mesh.point(mesh.to_vertex_handle(vj_vi));
	HalfedgeHandle vi_va = mesh.next_halfedge_handle(vj_vi);
	Point pa = mesh.point(mesh.to_vertex_handle(vi_va));
	double a = angle_between(pi, pa, pj);
	double b = angle_between(pj, pb, pi);
	return cotan(a) + cotan(b);
}


/* Calcule l'aire de chaque face et la stoque dans une propriété
 * "face_area" du maillage. */
void compute_face_areas(MyMesh &mesh) {
	auto area = OpenMesh::getOrMakeProperty<FaceHandle, double>
		(mesh, "face_area");
	for (FaceHandle fh : mesh.faces()) {
		MyMesh::FaceVertexIter fv_it = mesh.fv_iter(fh);
		Point pi = mesh.point(*fv_it);
		Point pj = mesh.point(*++fv_it);
		Point pk = mesh.point(*++fv_it);
		area[fh] = triangle_area(pi, pj, pk);
	}
}


/* Calcule l'aire barycentrique incidente à vert. */
double barycentric_vertex_area(MyMesh &mesh, VertexHandle vert) {
	auto area = OpenMesh::getProperty<FaceHandle, double>(mesh, "face_area");
	double sum = 0;
	for (FaceHandle fh : mesh.vf_range(vert)) {
		sum += area[fh];
	}
	return sum / 3;
}


/* Fonction de masse pour le laplacien cotangent. */
double cotangent_mass(MyMesh &mesh, VertexHandle vi) {
	double area = barycentric_vertex_area(mesh, vi);
	return 1. / (2 * area);
}


/* Calcule la matrice laplacienne avec les fonctions de poids d'arête
 * et de masse de sommet spécifiées par l'utilisatrice. */
Eigen::SparseMatrix<qreal> laplacian_matrix(MyMesh &mesh, double (*edge_weight)(MyMesh &, HalfedgeHandle), double (*vertex_mass)(MyMesh &, VertexHandle)) {
	compute_face_areas(mesh);
	size_t n_verts = mesh.n_vertices();
	Eigen::SparseMatrix<double> weight(n_verts, n_verts);
	Eigen::SparseMatrix<double> mass(n_verts, n_verts);
	for (VertexHandle vi : mesh.vertices()) {
		if (mesh.is_boundary(vi)) continue;
		double sum = 0;
		for (HalfedgeHandle vi_vj : mesh.voh_range(vi)) {
			// For each outgoing halfedge
			VertexHandle vj = mesh.to_vertex_handle(vi_vj);
			double halfedge_weight = edge_weight(mesh, vi_vj);
			weight.insert(vi.idx(), vj.idx()) = halfedge_weight;
			sum -= halfedge_weight;
		}
		// weight(i, i) = -Σ(j in N1(i)) weight(i, j)
		weight.insert(vi.idx(), vi.idx()) = sum;

		mass.insert(vi.idx(), vi.idx()) = vertex_mass(mesh, vi);
	}
	return mass * weight;
}


/* Adoucissement du maillage. */
void smooth(MyMesh &mesh, SmoothingMethod method, double cotan_factor) {
	// Calcul de la matrice laplacienne en fonction de la méthode choisie.
	double factor;
	Eigen::SparseMatrix<qreal> laplacian;
	if (method == SmoothingMethod::COTANGENT) {
		factor = cotan_factor;
		laplacian = laplacian_matrix(mesh, cotangent_weight, cotangent_mass);
	} else {
		factor = 1;
		laplacian = laplacian_matrix(mesh, uniform_weight, uniform_mass);
	}

	// laplacian = laplacian * laplacian; // Mise au carré.

	// Transfert des coordonées de chaque point dans une matrice.
	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];
	}

	// Application du laplacien pour calculer les décalages.
	X = laplacian * X;
	Y = laplacian * Y;
	Z = laplacian * Z;

	// Application des décalages.
	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 * factor);
	}
}