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


void Courbures::normales_locales() {
    _mesh.request_vertex_normals();
    size_t i;
    for (VertexHandle vh : _mesh.vertices()) {
		MyMesh::Normal normal(0,0,0);
        i = 0;
		for (MyMesh::FaceHandle vf : _mesh.vf_range(vh)) {
            i++;
            normal += _mesh.calc_face_normal(vf);
        }
        if (i != 0) {
            normal /= i;
            normal /= norm(normal);
        }
        _mesh.set_normal(vh, normal);
    }
}


void Courbures::get_two_neighborhood(std::vector<MyMesh::VertexHandle> &out,
									 const MyMesh::VertexHandle vh) {
	auto flag_prop =
		OpenMesh::getOrMakeProperty<VertexHandle, bool>(_mesh, "vprop_flag");
	flag_prop.set_range(_mesh.vertices_begin(), _mesh.vertices_end(), false);

	// Parcours du 1-anneau
	flag_prop[vh] = true;
    for(VertexHandle vv : _mesh.vv_range(vh)) {
        out.push_back(vv); // ajout du point à la liste
        flag_prop[vv] = true;
    }

	if (out.size() >= 5) return;

    // Parcours du 1-anneau de chaque sommet du 1-anneau
	size_t old_size = out.size();
    for (size_t i = 0; i < old_size; i++) {
        for (VertexHandle vv : _mesh.vv_range(out[i])) {
            if (!flag_prop[vv]) {
				out.push_back(vv);
				flag_prop[vv] = true;
			}
        }
    }
}


QuadPatch Courbures::fit_quad(MyMesh::VertexHandle vh) {
    static std::vector<MyMesh::VertexHandle> neigh;
	neigh.clear();
	get_two_neighborhood(neigh, vh);
    if (neigh.size() < 5) throw "Quad fitting: not enough neighbors";

    // Calcul de la matrice de changement de base
	Eigen::Vector3d Oz(0,0,1);
	Eigen::Vector3d axis = _mesh.normal(vh).cross(Oz);
    double sina = axis.norm();
	double cosa = _mesh.normal(vh).dot(Oz);
	double angle;
    if (sina >= 0) {
        angle = acos(cosa);
	} else {
        angle = -acos(cosa);
	}
    axis = axis.normalized();
    Eigen::AngleAxisd rot(angle, axis);
    Eigen::Translation3d trans(-_mesh.point(vh));
    Eigen::Transform<double, 3, Eigen::Affine> ch_base = rot * trans;

    // Calcul de la matrice / vecteur de moindres carrés linéaires (Eigen)
	Eigen::MatrixXd A(neigh.size(), 5);
	Eigen::VectorXd B(neigh.size());
	for(size_t i = 0; i < neigh.size(); i++) {
		MyMesh::Point point = _mesh.point(neigh[i]);
		point = ch_base * point; // Application du changement de base
		B[i] = -point[2]; // -zi
		A(i, 0) = point[0] * point[0]; // xi^2
		A(i, 1) = point[0] * point[1]; // xi*yi
		A(i, 2) = point[1] * point[1]; // yi^2
		A(i, 3) = point[0]; // xi
		A(i, 4) = point[1]; // yi
	}

	// Résolution aux moindres carrés par SVD
	Eigen::VectorXd coef(5);
	coef = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
	return QuadPatch(coef, ch_base);
}


void Courbures::compute_KH() {
    _mesh.add_property(vprop_K,  "vprop_K");
    _mesh.add_property(vprop_H,  "vprop_H");
	_mesh.add_property(vprop_quad, "vprop_quad");

    for (VertexHandle vh : _mesh.vertices()) {
        QuadPatch q = fit_quad(vh);
		_mesh.property(vprop_quad, vh) = q;

		// K = det (K_P) = 4 a_0 a_2 - a_1 ^ 2
        _mesh.property(vprop_K, vh) = 4 * q[0] * q[2] - q[1]*q[1];

		// H = Tr (K_P) = a_0 + a_2
        _mesh.property(vprop_H, vh) = q[0] + q[2];
    }
}


void Courbures::set_fixed_colors() {
    size_t i = 0;
    for (VertexHandle vh : _mesh.vertices()) {
        if (i % 2)
            _mesh.set_color(vh, MyMesh::Color(0, 1, 0));
        else
            _mesh.set_color(vh, MyMesh::Color(0, 0, 1));
		i++;
    }
}


void Courbures::set_K_colors() {
	auto prop = _mesh.property(vprop_K);
	auto prop_data = prop.data_vector();
    double avg = average(prop_data.begin(), prop_data.end());
	double stddev = standard_deviation
		(prop_data.begin(), prop_data.end(), avg);
    std::cout << "K mean : " << avg << " - sigma : " << stddev << std::endl;

    for (VertexHandle vh : _mesh.vertices()) {
        double tmp = _mesh.property(vprop_K, vh) - avg;
		tmp = clamp<double>(tmp, -stddev, +stddev);
		tmp = (tmp + stddev) / (2 * stddev);
		_mesh.set_color(vh, MyMesh::Color(tmp, .23, .5));
    }
}