//== INCLUDES =================================================================


#include "Energy_term_pca_cpc_ps2mesh_skinned.h"

#include "../settings.h"

#include <graphene/geometry/bary_coord.h>

#include <graphene/surface_mesh/algorithms/templatefitting/utility/my_helper.h>


//== NAMESPACE ================================================================


namespace graphene {
namespace surface_mesh {


//== IMPLEMENTATION ===========================================================


Energy_term_pca_cpc_ps2mesh_skinned::
Energy_term_pca_cpc_ps2mesh_skinned(const Energy_term_data& energy_term_data, double weight)
: Energy_term(energy_term_data, weight, "PCA CPC PointSet-to-Mesh Skinned")
{
}


//-----------------------------------------------------------------------------


Energy_term_pca_cpc_ps2mesh_skinned::
~Energy_term_pca_cpc_ps2mesh_skinned()
{}

//-----------------------------------------------------------------------------

void
Energy_term_pca_cpc_ps2mesh_skinned::
pre_minimize_action()
{
    if (!energy_term_data_.character)
    {
        std::cerr << "[ERROR] - Energy_term_pca_cpc_ps2mesh_skinned::!energy_term_data_.character - EXIT!" << std::endl;
        exit(1);
    }

    const std::vector<Mat4f>& skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;
    Surface_mesh::Vertex_property<Vec4f> vdepends = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    Surface_mesh::Vertex_property<Vec4f> vweights = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    Surface_mesh::Vertex_property<Vec4f> vdepends2 = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    Surface_mesh::Vertex_property<Vec4f> vweights2 = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");

    if (!(vdepends && vweights && vdepends2 && vweights2) || skinning_tpose_to_scanpose.size() == 0)
    {
        std::cerr << "[ERROR] - Energy_term_pca_cpc_ps2mesh_skinned::pre_minimize_action() - skinning_tpose_to_scanpose.size(): " << skinning_tpose_to_scanpose.size() << std::endl;
        std::cerr << "[ERROR] - Energy_term_pca_cpc_ps2mesh_skinned::pre_minimize_action() - EXIT!" << std::endl;
        exit(1);
    }

    const PCA& pca = energy_term_data_.pca;

    // compute T_j*H_i for a convenient access (for all joints j we store a (3*#verts X d)-matrix)
    // T_j are skinning-matrices
    // H_i are the three rows corresponding to vertex i
    Tj_times_Hi_.clear();
    for (unsigned int i = 0; i < skinning_tpose_to_scanpose.size(); ++i)
    {
        const Mat4f& current_skinning_mat = skinning_tpose_to_scanpose[i]; // current 4x4 skinning matrix

        Eigen::MatrixXd temp_mat(pca.P_pca_.rows(), pca.P_pca_.cols());

        for (unsigned int j = 0; j < energy_term_data_.template_mesh->n_vertices(); ++j)
        {
            // compute and set sub-matrix
            for (unsigned int k = 0; k < pca.P_pca_.cols(); ++k)
            {
                Vec3f p(pca.P_pca_(3*j+0, k), pca.P_pca_(3*j+1, k), pca.P_pca_(3*j+2, k));

                // current skinning_mat * sub-matrix
                Vec3f trans_p = affine_transform(current_skinning_mat, p);

                temp_mat(3*j+0, k) = trans_p[0];
                temp_mat(3*j+1, k) = trans_p[1];
                temp_mat(3*j+2, k) = trans_p[2];
            }
        }

        Tj_times_Hi_.push_back(temp_mat);
    }

    // also compute T_j*pca_mean for a convenient access (for all joints j we store a (3*#verts)-vector)
    Tj_times_pcamean_.clear();
    for (unsigned int i = 0; i < skinning_tpose_to_scanpose.size(); ++i)
    {
        const Mat4f& current_skinning_mat = skinning_tpose_to_scanpose[i]; // current 4x4 skinning matrix

        Eigen::VectorXd temp_vec(pca.m_pca_.rows());

        for (unsigned int j = 0; j < energy_term_data_.template_mesh->n_vertices(); ++j)
        {
            Vec3f p(pca.m_pca_(3*j+0), pca.m_pca_(3*j+1), pca.m_pca_(3*j+2));

            // current skinning_mat * sub-matrix
            Vec3f trans_p = affine_transform(current_skinning_mat, p);

            temp_vec(3*j+0) = trans_p[0];
            temp_vec(3*j+1) = trans_p[1];
            temp_vec(3*j+2) = trans_p[2];
        }

        Tj_times_pcamean_.push_back(temp_vec);
    }
}

//-----------------------------------------------------------------------------


double
Energy_term_pca_cpc_ps2mesh_skinned::
evaluate()
{
    const std::vector<Correspondence>& correspondences = energy_term_data_.correspondences;
    const PCA& pca = energy_term_data_.pca;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    auto template_points = template_mesh->get_vertex_property<Point>("v:point");

    Surface_mesh::Vertex_property<Vec4f> vdepends = template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    Surface_mesh::Vertex_property<Vec4f> vweights = template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    Surface_mesh::Vertex_property<Vec4f> vdepends2 = template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    Surface_mesh::Vertex_property<Vec4f> vweights2 = template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    if (!(vdepends && vweights && vdepends2 && vweights2) || Tj_times_Hi_.size() == 0 || Tj_times_pcamean_.size() == 0)
    {
        std::cerr << "DEBUG - Tj_times_Hi_.size(): " << Tj_times_Hi_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_pcamean_.size(): " << Tj_times_pcamean_.size() << std::endl;
        std::cerr << "[ERROR] - Energy_term_pca_cpc_ps2mesh_skinned::evaluate() - EXIT!" << std::endl;
        exit(1);
    }

    double error_reg_pca_cpc_ps2mesh = 0.0;

    const unsigned int C = correspondences.size();

    for (unsigned int i = 0; i < C; ++i)
    {
        if ( correspondences[i].constr_dir == corresp_dir_mesh2ps )
        {
            std::cerr << "BAD ERROR IN Energy_term_pca_cpc_ps2mesh_skinned::evaluate() - mesh2ps not implemented" << std::endl;
            exit(1);
        }

        // get vertices of closest triangle
        auto fvit = template_mesh->vertices( correspondences[i].face );
        const auto v0 = *fvit;
        const auto v1 = *(++fvit);
        const auto v2 = *(++fvit);
        const Point p0 = template_points[v0];
        const Point p1 = template_points[v1];
        const Point p2 = template_points[v2];
        // get barycentric coordinates
        const Point bary = graphene::geometry::barycentric_coordinates( correspondences[i].on_template, p0, p1, p2 );

        int idxA = v0.idx();
        int idxB = v1.idx();
        int idxC = v2.idx();

        int v0_dep0 = (int)vdepends[v0][0];
        float v0_w0 = vweights[v0][0];
        int v0_dep1 = (int)vdepends[v0][1];
        float v0_w1 = vweights[v0][1];
        int v0_dep2 = (int)vdepends[v0][2];
        float v0_w2 = vweights[v0][2];
        int v0_dep3 = (int)vdepends[v0][3];
        float v0_w3 = vweights[v0][3];
        int v0_dep4 = (int)vdepends2[v0][0];
        float v0_w4 = vweights2[v0][0];
        int v0_dep5 = (int)vdepends2[v0][1];
        float v0_w5 = vweights2[v0][1];
        int v0_dep6 = (int)vdepends2[v0][2];
        float v0_w6 = vweights2[v0][2];
        int v0_dep7 = (int)vdepends2[v0][3];
        float v0_w7 = vweights2[v0][3];

        int v1_dep0 = (int)vdepends[v1][0];
        float v1_w0 = vweights[v1][0];
        int v1_dep1 = (int)vdepends[v1][1];
        float v1_w1 = vweights[v1][1];
        int v1_dep2 = (int)vdepends[v1][2];
        float v1_w2 = vweights[v1][2];
        int v1_dep3 = (int)vdepends[v1][3];
        float v1_w3 = vweights[v1][3];
        int v1_dep4 = (int)vdepends2[v1][0];
        float v1_w4 = vweights2[v1][0];
        int v1_dep5 = (int)vdepends2[v1][1];
        float v1_w5 = vweights2[v1][1];
        int v1_dep6 = (int)vdepends2[v1][2];
        float v1_w6 = vweights2[v1][2];
        int v1_dep7 = (int)vdepends2[v1][3];
        float v1_w7 = vweights2[v1][3];

        int v2_dep0 = (int)vdepends[v2][0];
        float v2_w0 = vweights[v2][0];
        int v2_dep1 = (int)vdepends[v2][1];
        float v2_w1 = vweights[v2][1];
        int v2_dep2 = (int)vdepends[v2][2];
        float v2_w2 = vweights[v2][2];
        int v2_dep3 = (int)vdepends[v2][3];
        float v2_w3 = vweights[v2][3];
        int v2_dep4 = (int)vdepends2[v2][0];
        float v2_w4 = vweights2[v2][0];
        int v2_dep5 = (int)vdepends2[v2][1];
        float v2_w5 = vweights2[v2][1];
        int v2_dep6 = (int)vdepends2[v2][2];
        float v2_w6 = vweights2[v2][2];
        int v2_dep7 = (int)vdepends2[v2][3];
        float v2_w7 = vweights2[v2][3];

        Eigen::MatrixXd part_mat(3, pca.P_pca_.cols());
        part_mat.setZero();
        // vertex 1
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            part_mat(0, j)  = bary[0]*v0_w0*Tj_times_Hi_[v0_dep0](3*idxA+0, j);
            part_mat(1, j)  = bary[0]*v0_w0*Tj_times_Hi_[v0_dep0](3*idxA+1, j);
            part_mat(2, j)  = bary[0]*v0_w0*Tj_times_Hi_[v0_dep0](3*idxA+2, j);

            part_mat(0, j) += bary[0]*v0_w1*Tj_times_Hi_[v0_dep1](3*idxA+0, j);
            part_mat(1, j) += bary[0]*v0_w1*Tj_times_Hi_[v0_dep1](3*idxA+1, j);
            part_mat(2, j) += bary[0]*v0_w1*Tj_times_Hi_[v0_dep1](3*idxA+2, j);

            part_mat(0, j) += bary[0]*v0_w2*Tj_times_Hi_[v0_dep2](3*idxA+0, j);
            part_mat(1, j) += bary[0]*v0_w2*Tj_times_Hi_[v0_dep2](3*idxA+1, j);
            part_mat(2, j) += bary[0]*v0_w2*Tj_times_Hi_[v0_dep2](3*idxA+2, j);

            part_mat(0, j) += bary[0]*v0_w3*Tj_times_Hi_[v0_dep3](3*idxA+0, j);
            part_mat(1, j) += bary[0]*v0_w3*Tj_times_Hi_[v0_dep3](3*idxA+1, j);
            part_mat(2, j) += bary[0]*v0_w3*Tj_times_Hi_[v0_dep3](3*idxA+2, j);

            part_mat(0, j) += bary[0]*v0_w4*Tj_times_Hi_[v0_dep4](3*idxA+0, j);
            part_mat(1, j) += bary[0]*v0_w4*Tj_times_Hi_[v0_dep4](3*idxA+1, j);
            part_mat(2, j) += bary[0]*v0_w4*Tj_times_Hi_[v0_dep4](3*idxA+2, j);

            part_mat(0, j) += bary[0]*v0_w5*Tj_times_Hi_[v0_dep5](3*idxA+0, j);
            part_mat(1, j) += bary[0]*v0_w5*Tj_times_Hi_[v0_dep5](3*idxA+1, j);
            part_mat(2, j) += bary[0]*v0_w5*Tj_times_Hi_[v0_dep5](3*idxA+2, j);

            part_mat(0, j) += bary[0]*v0_w6*Tj_times_Hi_[v0_dep6](3*idxA+0, j);
            part_mat(1, j) += bary[0]*v0_w6*Tj_times_Hi_[v0_dep6](3*idxA+1, j);
            part_mat(2, j) += bary[0]*v0_w6*Tj_times_Hi_[v0_dep6](3*idxA+2, j);

            part_mat(0, j) += bary[0]*v0_w7*Tj_times_Hi_[v0_dep7](3*idxA+0, j);
            part_mat(1, j) += bary[0]*v0_w7*Tj_times_Hi_[v0_dep7](3*idxA+1, j);
            part_mat(2, j) += bary[0]*v0_w7*Tj_times_Hi_[v0_dep7](3*idxA+2, j);
        }
        // vertex 2
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            part_mat(0, j) += bary[1]*v1_w0*Tj_times_Hi_[v1_dep0](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w0*Tj_times_Hi_[v1_dep0](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w0*Tj_times_Hi_[v1_dep0](3*idxB+2, j);

            part_mat(0, j) += bary[1]*v1_w1*Tj_times_Hi_[v1_dep1](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w1*Tj_times_Hi_[v1_dep1](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w1*Tj_times_Hi_[v1_dep1](3*idxB+2, j);

            part_mat(0, j) += bary[1]*v1_w2*Tj_times_Hi_[v1_dep2](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w2*Tj_times_Hi_[v1_dep2](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w2*Tj_times_Hi_[v1_dep2](3*idxB+2, j);

            part_mat(0, j) += bary[1]*v1_w3*Tj_times_Hi_[v1_dep3](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w3*Tj_times_Hi_[v1_dep3](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w3*Tj_times_Hi_[v1_dep3](3*idxB+2, j);

            part_mat(0, j) += bary[1]*v1_w4*Tj_times_Hi_[v1_dep4](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w4*Tj_times_Hi_[v1_dep4](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w4*Tj_times_Hi_[v1_dep4](3*idxB+2, j);

            part_mat(0, j) += bary[1]*v1_w5*Tj_times_Hi_[v1_dep5](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w5*Tj_times_Hi_[v1_dep5](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w5*Tj_times_Hi_[v1_dep5](3*idxB+2, j);

            part_mat(0, j) += bary[1]*v1_w6*Tj_times_Hi_[v1_dep6](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w6*Tj_times_Hi_[v1_dep6](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w6*Tj_times_Hi_[v1_dep6](3*idxB+2, j);

            part_mat(0, j) += bary[1]*v1_w7*Tj_times_Hi_[v1_dep7](3*idxB+0, j);
            part_mat(1, j) += bary[1]*v1_w7*Tj_times_Hi_[v1_dep7](3*idxB+1, j);
            part_mat(2, j) += bary[1]*v1_w7*Tj_times_Hi_[v1_dep7](3*idxB+2, j);
        }
        // vertex 3
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            part_mat(0, j) += bary[2]*v2_w0*Tj_times_Hi_[v2_dep0](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w0*Tj_times_Hi_[v2_dep0](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w0*Tj_times_Hi_[v2_dep0](3*idxC+2, j);

            part_mat(0, j) += bary[2]*v2_w1*Tj_times_Hi_[v2_dep1](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w1*Tj_times_Hi_[v2_dep1](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w1*Tj_times_Hi_[v2_dep1](3*idxC+2, j);

            part_mat(0, j) += bary[2]*v2_w2*Tj_times_Hi_[v2_dep2](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w2*Tj_times_Hi_[v2_dep2](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w2*Tj_times_Hi_[v2_dep2](3*idxC+2, j);

            part_mat(0, j) += bary[2]*v2_w3*Tj_times_Hi_[v2_dep3](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w3*Tj_times_Hi_[v2_dep3](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w3*Tj_times_Hi_[v2_dep3](3*idxC+2, j);

            part_mat(0, j) += bary[2]*v2_w4*Tj_times_Hi_[v2_dep4](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w4*Tj_times_Hi_[v2_dep4](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w4*Tj_times_Hi_[v2_dep4](3*idxC+2, j);

            part_mat(0, j) += bary[2]*v2_w5*Tj_times_Hi_[v2_dep5](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w5*Tj_times_Hi_[v2_dep5](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w5*Tj_times_Hi_[v2_dep5](3*idxC+2, j);

            part_mat(0, j) += bary[2]*v2_w6*Tj_times_Hi_[v2_dep6](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w6*Tj_times_Hi_[v2_dep6](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w6*Tj_times_Hi_[v2_dep6](3*idxC+2, j);

            part_mat(0, j) += bary[2]*v2_w7*Tj_times_Hi_[v2_dep7](3*idxC+0, j);
            part_mat(1, j) += bary[2]*v2_w7*Tj_times_Hi_[v2_dep7](3*idxC+1, j);
            part_mat(2, j) += bary[2]*v2_w7*Tj_times_Hi_[v2_dep7](3*idxC+2, j);
        }

        Vec3f part_vec(0);
        // vertex 1
        part_vec[0]  = bary[0]*v0_w0*Tj_times_pcamean_[v0_dep0](3*idxA+0);
        part_vec[1]  = bary[0]*v0_w0*Tj_times_pcamean_[v0_dep0](3*idxA+1);
        part_vec[2]  = bary[0]*v0_w0*Tj_times_pcamean_[v0_dep0](3*idxA+2);
        part_vec[0] += bary[0]*v0_w1*Tj_times_pcamean_[v0_dep1](3*idxA+0);
        part_vec[1] += bary[0]*v0_w1*Tj_times_pcamean_[v0_dep1](3*idxA+1);
        part_vec[2] += bary[0]*v0_w1*Tj_times_pcamean_[v0_dep1](3*idxA+2);
        part_vec[0] += bary[0]*v0_w2*Tj_times_pcamean_[v0_dep2](3*idxA+0);
        part_vec[1] += bary[0]*v0_w2*Tj_times_pcamean_[v0_dep2](3*idxA+1);
        part_vec[2] += bary[0]*v0_w2*Tj_times_pcamean_[v0_dep2](3*idxA+2);
        part_vec[0] += bary[0]*v0_w3*Tj_times_pcamean_[v0_dep3](3*idxA+0);
        part_vec[1] += bary[0]*v0_w3*Tj_times_pcamean_[v0_dep3](3*idxA+1);
        part_vec[2] += bary[0]*v0_w3*Tj_times_pcamean_[v0_dep3](3*idxA+2);
        part_vec[0] += bary[0]*v0_w4*Tj_times_pcamean_[v0_dep4](3*idxA+0);
        part_vec[1] += bary[0]*v0_w4*Tj_times_pcamean_[v0_dep4](3*idxA+1);
        part_vec[2] += bary[0]*v0_w4*Tj_times_pcamean_[v0_dep4](3*idxA+2);
        part_vec[0] += bary[0]*v0_w5*Tj_times_pcamean_[v0_dep5](3*idxA+0);
        part_vec[1] += bary[0]*v0_w5*Tj_times_pcamean_[v0_dep5](3*idxA+1);
        part_vec[2] += bary[0]*v0_w5*Tj_times_pcamean_[v0_dep5](3*idxA+2);
        part_vec[0] += bary[0]*v0_w6*Tj_times_pcamean_[v0_dep6](3*idxA+0);
        part_vec[1] += bary[0]*v0_w6*Tj_times_pcamean_[v0_dep6](3*idxA+1);
        part_vec[2] += bary[0]*v0_w6*Tj_times_pcamean_[v0_dep6](3*idxA+2);
        part_vec[0] += bary[0]*v0_w7*Tj_times_pcamean_[v0_dep7](3*idxA+0);
        part_vec[1] += bary[0]*v0_w7*Tj_times_pcamean_[v0_dep7](3*idxA+1);
        part_vec[2] += bary[0]*v0_w7*Tj_times_pcamean_[v0_dep7](3*idxA+2);
        // vertex 2
        part_vec[0] += bary[1]*v1_w0*Tj_times_pcamean_[v1_dep0](3*idxB+0);
        part_vec[1] += bary[1]*v1_w0*Tj_times_pcamean_[v1_dep0](3*idxB+1);
        part_vec[2] += bary[1]*v1_w0*Tj_times_pcamean_[v1_dep0](3*idxB+2);
        part_vec[0] += bary[1]*v1_w1*Tj_times_pcamean_[v1_dep1](3*idxB+0);
        part_vec[1] += bary[1]*v1_w1*Tj_times_pcamean_[v1_dep1](3*idxB+1);
        part_vec[2] += bary[1]*v1_w1*Tj_times_pcamean_[v1_dep1](3*idxB+2);
        part_vec[0] += bary[1]*v1_w2*Tj_times_pcamean_[v1_dep2](3*idxB+0);
        part_vec[1] += bary[1]*v1_w2*Tj_times_pcamean_[v1_dep2](3*idxB+1);
        part_vec[2] += bary[1]*v1_w2*Tj_times_pcamean_[v1_dep2](3*idxB+2);
        part_vec[0] += bary[1]*v1_w3*Tj_times_pcamean_[v1_dep3](3*idxB+0);
        part_vec[1] += bary[1]*v1_w3*Tj_times_pcamean_[v1_dep3](3*idxB+1);
        part_vec[2] += bary[1]*v1_w3*Tj_times_pcamean_[v1_dep3](3*idxB+2);
        part_vec[0] += bary[1]*v1_w4*Tj_times_pcamean_[v1_dep4](3*idxB+0);
        part_vec[1] += bary[1]*v1_w4*Tj_times_pcamean_[v1_dep4](3*idxB+1);
        part_vec[2] += bary[1]*v1_w4*Tj_times_pcamean_[v1_dep4](3*idxB+2);
        part_vec[0] += bary[1]*v1_w5*Tj_times_pcamean_[v1_dep5](3*idxB+0);
        part_vec[1] += bary[1]*v1_w5*Tj_times_pcamean_[v1_dep5](3*idxB+1);
        part_vec[2] += bary[1]*v1_w5*Tj_times_pcamean_[v1_dep5](3*idxB+2);
        part_vec[0] += bary[1]*v1_w6*Tj_times_pcamean_[v1_dep6](3*idxB+0);
        part_vec[1] += bary[1]*v1_w6*Tj_times_pcamean_[v1_dep6](3*idxB+1);
        part_vec[2] += bary[1]*v1_w6*Tj_times_pcamean_[v1_dep6](3*idxB+2);
        part_vec[0] += bary[1]*v1_w7*Tj_times_pcamean_[v1_dep7](3*idxB+0);
        part_vec[1] += bary[1]*v1_w7*Tj_times_pcamean_[v1_dep7](3*idxB+1);
        part_vec[2] += bary[1]*v1_w7*Tj_times_pcamean_[v1_dep7](3*idxB+2);
        // vertex 3
        part_vec[0] += bary[2]*v2_w0*Tj_times_pcamean_[v2_dep0](3*idxC+0);
        part_vec[1] += bary[2]*v2_w0*Tj_times_pcamean_[v2_dep0](3*idxC+1);
        part_vec[2] += bary[2]*v2_w0*Tj_times_pcamean_[v2_dep0](3*idxC+2);
        part_vec[0] += bary[2]*v2_w1*Tj_times_pcamean_[v2_dep1](3*idxC+0);
        part_vec[1] += bary[2]*v2_w1*Tj_times_pcamean_[v2_dep1](3*idxC+1);
        part_vec[2] += bary[2]*v2_w1*Tj_times_pcamean_[v2_dep1](3*idxC+2);
        part_vec[0] += bary[2]*v2_w2*Tj_times_pcamean_[v2_dep2](3*idxC+0);
        part_vec[1] += bary[2]*v2_w2*Tj_times_pcamean_[v2_dep2](3*idxC+1);
        part_vec[2] += bary[2]*v2_w2*Tj_times_pcamean_[v2_dep2](3*idxC+2);
        part_vec[0] += bary[2]*v2_w3*Tj_times_pcamean_[v2_dep3](3*idxC+0);
        part_vec[1] += bary[2]*v2_w3*Tj_times_pcamean_[v2_dep3](3*idxC+1);
        part_vec[2] += bary[2]*v2_w3*Tj_times_pcamean_[v2_dep3](3*idxC+2);
        part_vec[0] += bary[2]*v2_w4*Tj_times_pcamean_[v2_dep4](3*idxC+0);
        part_vec[1] += bary[2]*v2_w4*Tj_times_pcamean_[v2_dep4](3*idxC+1);
        part_vec[2] += bary[2]*v2_w4*Tj_times_pcamean_[v2_dep4](3*idxC+2);
        part_vec[0] += bary[2]*v2_w5*Tj_times_pcamean_[v2_dep5](3*idxC+0);
        part_vec[1] += bary[2]*v2_w5*Tj_times_pcamean_[v2_dep5](3*idxC+1);
        part_vec[2] += bary[2]*v2_w5*Tj_times_pcamean_[v2_dep5](3*idxC+2);
        part_vec[0] += bary[2]*v2_w6*Tj_times_pcamean_[v2_dep6](3*idxC+0);
        part_vec[1] += bary[2]*v2_w6*Tj_times_pcamean_[v2_dep6](3*idxC+1);
        part_vec[2] += bary[2]*v2_w6*Tj_times_pcamean_[v2_dep6](3*idxC+2);
        part_vec[0] += bary[2]*v2_w7*Tj_times_pcamean_[v2_dep7](3*idxC+0);
        part_vec[1] += bary[2]*v2_w7*Tj_times_pcamean_[v2_dep7](3*idxC+1);
        part_vec[2] += bary[2]*v2_w7*Tj_times_pcamean_[v2_dep7](3*idxC+2);

        Point the_point(0.0, 0.0, 0.0);
        for (unsigned int j = 0; j < 3; ++j)
        {
            for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
            {
                the_point[j] += part_mat(j, dcount) * pca.d_pca_(dcount) + part_vec[j];
            }
        }

        double u = norm( the_point - correspondences[i].on_ps );
        if ( robustness_parameters_.solver_type == SOLVER_TYPE_L2 )
        {
            error_reg_pca_cpc_ps2mesh += u * u;
        }
        else
        {
            std::cerr << "[ERROR] in 'Energy_term_pca_cpc_ps2mesh_skinned::evaluate()' robust function not supported! Aborting..." << std::endl;
            exit(1);
        }
    }
    error_reg_pca_cpc_ps2mesh *= (weight_ / C);

    return error_reg_pca_cpc_ps2mesh;
}


//-----------------------------------------------------------------------------


void
Energy_term_pca_cpc_ps2mesh_skinned::
add_rows_big_lse( std::vector<Tripl>& coeffs,
                  Eigen::VectorXd& b,
                  unsigned int& r)
{
    //std::cerr << "[INFO] in: Energy_term_pca_cpc_ps2mesh_skinned::add_rows_big_lse(): weight_: " << weight_ << std::endl;

    //choose either the skinnend correspondences or the "normal" ones based on whether skinned correspondences exist or not
    const std::vector<Correspondence>& correspondences = energy_term_data_.correspondences;

    const Column_offsets& column_offsets = energy_term_data_.column_offsets;
    const unsigned int offset_nrd_w_pca = column_offsets.offset_nrd_w_pca;
    const PCA& pca = energy_term_data_.pca;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    auto template_points = template_mesh->get_vertex_property<Point>("v:point");

    Surface_mesh::Vertex_property<Vec4f> vdepends = template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    Surface_mesh::Vertex_property<Vec4f> vweights = template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    Surface_mesh::Vertex_property<Vec4f> vdepends2 = template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    Surface_mesh::Vertex_property<Vec4f> vweights2 = template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    if (!(vdepends && vweights && vdepends2 && vweights2) || Tj_times_Hi_.size() == 0 || Tj_times_pcamean_.size() == 0)
    {
        std::cerr << "DEBUG - Tj_times_Hi_.size(): " << Tj_times_Hi_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_pcamean_.size(): " << Tj_times_pcamean_.size() << std::endl;
        std::cerr << "[ERROR] - Energy_term_pca_landmarks_skinned::add_rows_big_lse() - EXIT!" << std::endl;
        exit(1);
    }

    const unsigned int C = correspondences.size();

    const double weight_model = sqrt(weight_ / C);
    //std::cout << "[INFO] Energy_term_pca_cpc_ps2mesh_skinned::add_rows_big_lse(): weight_model: " << weight_model << std::endl;
    for (unsigned int i = 0; i < C; ++i)
    {
        if (correspondences[i].constr_dir == graphene::surface_mesh::corresp_dir_mesh2ps ||
            correspondences[i].constr_dir == graphene::surface_mesh::corresp_dir_mesh2ps_and_ps2mesh ) // TODO EVTL. WOANDERS HIN
        {
            std::cerr << "[ERROR] in 'Energy_term_pca_cpc_ps2mesh_skinned::add_rows_big_lse()' bad exception! Aborting..." << std::endl;
            exit(1);
        }

        // get vertices of closest triangle
        auto fvit = template_mesh->vertices( correspondences[i].face );
        const auto v0 = *fvit;
        const auto v1 = *(++fvit);
        const auto v2 = *(++fvit);
        const Point p0 = template_points[v0];
        const Point p1 = template_points[v1];
        const Point p2 = template_points[v2];
        // get barycentric coordinates
        const Point bary = correspondences[i].on_template_bc;//graphene::geometry::barycentric_coordinates( correspondences[i].on_template, p0, p1, p2 );

        int idxA = v0.idx();
        int idxB = v1.idx();
        int idxC = v2.idx();

        int v0_dep0 = (int)vdepends[v0][0];
        float v0_w0 = vweights[v0][0];
        int v0_dep1 = (int)vdepends[v0][1];
        float v0_w1 = vweights[v0][1];
        int v0_dep2 = (int)vdepends[v0][2];
        float v0_w2 = vweights[v0][2];
        int v0_dep3 = (int)vdepends[v0][3];
        float v0_w3 = vweights[v0][3];
        int v0_dep4 = (int)vdepends2[v0][0];
        float v0_w4 = vweights2[v0][0];
        int v0_dep5 = (int)vdepends2[v0][1];
        float v0_w5 = vweights2[v0][1];
        int v0_dep6 = (int)vdepends2[v0][2];
        float v0_w6 = vweights2[v0][2];
        int v0_dep7 = (int)vdepends2[v0][3];
        float v0_w7 = vweights2[v0][3];

        int v1_dep0 = (int)vdepends[v1][0];
        float v1_w0 = vweights[v1][0];
        int v1_dep1 = (int)vdepends[v1][1];
        float v1_w1 = vweights[v1][1];
        int v1_dep2 = (int)vdepends[v1][2];
        float v1_w2 = vweights[v1][2];
        int v1_dep3 = (int)vdepends[v1][3];
        float v1_w3 = vweights[v1][3];
        int v1_dep4 = (int)vdepends2[v1][0];
        float v1_w4 = vweights2[v1][0];
        int v1_dep5 = (int)vdepends2[v1][1];
        float v1_w5 = vweights2[v1][1];
        int v1_dep6 = (int)vdepends2[v1][2];
        float v1_w6 = vweights2[v1][2];
        int v1_dep7 = (int)vdepends2[v1][3];
        float v1_w7 = vweights2[v1][3];

        int v2_dep0 = (int)vdepends[v2][0];
        float v2_w0 = vweights[v2][0];
        int v2_dep1 = (int)vdepends[v2][1];
        float v2_w1 = vweights[v2][1];
        int v2_dep2 = (int)vdepends[v2][2];
        float v2_w2 = vweights[v2][2];
        int v2_dep3 = (int)vdepends[v2][3];
        float v2_w3 = vweights[v2][3];
        int v2_dep4 = (int)vdepends2[v2][0];
        float v2_w4 = vweights2[v2][0];
        int v2_dep5 = (int)vdepends2[v2][1];
        float v2_w5 = vweights2[v2][1];
        int v2_dep6 = (int)vdepends2[v2][2];
        float v2_w6 = vweights2[v2][2];
        int v2_dep7 = (int)vdepends2[v2][3];
        float v2_w7 = vweights2[v2][3];

        // compute RHS part
        Vec3f rhs_part(0);
        // vertex 1
        rhs_part[0]  = bary[0]*v0_w0*Tj_times_pcamean_[v0_dep0](3*idxA+0);
        rhs_part[1]  = bary[0]*v0_w0*Tj_times_pcamean_[v0_dep0](3*idxA+1);
        rhs_part[2]  = bary[0]*v0_w0*Tj_times_pcamean_[v0_dep0](3*idxA+2);
        rhs_part[0] += bary[0]*v0_w1*Tj_times_pcamean_[v0_dep1](3*idxA+0);
        rhs_part[1] += bary[0]*v0_w1*Tj_times_pcamean_[v0_dep1](3*idxA+1);
        rhs_part[2] += bary[0]*v0_w1*Tj_times_pcamean_[v0_dep1](3*idxA+2);
        rhs_part[0] += bary[0]*v0_w2*Tj_times_pcamean_[v0_dep2](3*idxA+0);
        rhs_part[1] += bary[0]*v0_w2*Tj_times_pcamean_[v0_dep2](3*idxA+1);
        rhs_part[2] += bary[0]*v0_w2*Tj_times_pcamean_[v0_dep2](3*idxA+2);
        rhs_part[0] += bary[0]*v0_w3*Tj_times_pcamean_[v0_dep3](3*idxA+0);
        rhs_part[1] += bary[0]*v0_w3*Tj_times_pcamean_[v0_dep3](3*idxA+1);
        rhs_part[2] += bary[0]*v0_w3*Tj_times_pcamean_[v0_dep3](3*idxA+2);
        rhs_part[0] += bary[0]*v0_w4*Tj_times_pcamean_[v0_dep4](3*idxA+0);
        rhs_part[1] += bary[0]*v0_w4*Tj_times_pcamean_[v0_dep4](3*idxA+1);
        rhs_part[2] += bary[0]*v0_w4*Tj_times_pcamean_[v0_dep4](3*idxA+2);
        rhs_part[0] += bary[0]*v0_w5*Tj_times_pcamean_[v0_dep5](3*idxA+0);
        rhs_part[1] += bary[0]*v0_w5*Tj_times_pcamean_[v0_dep5](3*idxA+1);
        rhs_part[2] += bary[0]*v0_w5*Tj_times_pcamean_[v0_dep5](3*idxA+2);
        rhs_part[0] += bary[0]*v0_w6*Tj_times_pcamean_[v0_dep6](3*idxA+0);
        rhs_part[1] += bary[0]*v0_w6*Tj_times_pcamean_[v0_dep6](3*idxA+1);
        rhs_part[2] += bary[0]*v0_w6*Tj_times_pcamean_[v0_dep6](3*idxA+2);
        rhs_part[0] += bary[0]*v0_w7*Tj_times_pcamean_[v0_dep7](3*idxA+0);
        rhs_part[1] += bary[0]*v0_w7*Tj_times_pcamean_[v0_dep7](3*idxA+1);
        rhs_part[2] += bary[0]*v0_w7*Tj_times_pcamean_[v0_dep7](3*idxA+2);
        // vertex 2
        rhs_part[0] += bary[1]*v1_w0*Tj_times_pcamean_[v1_dep0](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w0*Tj_times_pcamean_[v1_dep0](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w0*Tj_times_pcamean_[v1_dep0](3*idxB+2);
        rhs_part[0] += bary[1]*v1_w1*Tj_times_pcamean_[v1_dep1](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w1*Tj_times_pcamean_[v1_dep1](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w1*Tj_times_pcamean_[v1_dep1](3*idxB+2);
        rhs_part[0] += bary[1]*v1_w2*Tj_times_pcamean_[v1_dep2](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w2*Tj_times_pcamean_[v1_dep2](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w2*Tj_times_pcamean_[v1_dep2](3*idxB+2);
        rhs_part[0] += bary[1]*v1_w3*Tj_times_pcamean_[v1_dep3](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w3*Tj_times_pcamean_[v1_dep3](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w3*Tj_times_pcamean_[v1_dep3](3*idxB+2);
        rhs_part[0] += bary[1]*v1_w4*Tj_times_pcamean_[v1_dep4](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w4*Tj_times_pcamean_[v1_dep4](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w4*Tj_times_pcamean_[v1_dep4](3*idxB+2);
        rhs_part[0] += bary[1]*v1_w5*Tj_times_pcamean_[v1_dep5](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w5*Tj_times_pcamean_[v1_dep5](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w5*Tj_times_pcamean_[v1_dep5](3*idxB+2);
        rhs_part[0] += bary[1]*v1_w6*Tj_times_pcamean_[v1_dep6](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w6*Tj_times_pcamean_[v1_dep6](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w6*Tj_times_pcamean_[v1_dep6](3*idxB+2);
        rhs_part[0] += bary[1]*v1_w7*Tj_times_pcamean_[v1_dep7](3*idxB+0);
        rhs_part[1] += bary[1]*v1_w7*Tj_times_pcamean_[v1_dep7](3*idxB+1);
        rhs_part[2] += bary[1]*v1_w7*Tj_times_pcamean_[v1_dep7](3*idxB+2);
        // vertex 3
        rhs_part[0] += bary[2]*v2_w0*Tj_times_pcamean_[v2_dep0](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w0*Tj_times_pcamean_[v2_dep0](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w0*Tj_times_pcamean_[v2_dep0](3*idxC+2);
        rhs_part[0] += bary[2]*v2_w1*Tj_times_pcamean_[v2_dep1](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w1*Tj_times_pcamean_[v2_dep1](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w1*Tj_times_pcamean_[v2_dep1](3*idxC+2);
        rhs_part[0] += bary[2]*v2_w2*Tj_times_pcamean_[v2_dep2](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w2*Tj_times_pcamean_[v2_dep2](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w2*Tj_times_pcamean_[v2_dep2](3*idxC+2);
        rhs_part[0] += bary[2]*v2_w3*Tj_times_pcamean_[v2_dep3](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w3*Tj_times_pcamean_[v2_dep3](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w3*Tj_times_pcamean_[v2_dep3](3*idxC+2);
        rhs_part[0] += bary[2]*v2_w4*Tj_times_pcamean_[v2_dep4](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w4*Tj_times_pcamean_[v2_dep4](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w4*Tj_times_pcamean_[v2_dep4](3*idxC+2);
        rhs_part[0] += bary[2]*v2_w5*Tj_times_pcamean_[v2_dep5](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w5*Tj_times_pcamean_[v2_dep5](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w5*Tj_times_pcamean_[v2_dep5](3*idxC+2);
        rhs_part[0] += bary[2]*v2_w6*Tj_times_pcamean_[v2_dep6](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w6*Tj_times_pcamean_[v2_dep6](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w6*Tj_times_pcamean_[v2_dep6](3*idxC+2);
        rhs_part[0] += bary[2]*v2_w7*Tj_times_pcamean_[v2_dep7](3*idxC+0);
        rhs_part[1] += bary[2]*v2_w7*Tj_times_pcamean_[v2_dep7](3*idxC+1);
        rhs_part[2] += bary[2]*v2_w7*Tj_times_pcamean_[v2_dep7](3*idxC+2);

        // compute LHS part
        Eigen::MatrixXd lhs_part(3, pca.P_pca_.cols());
        lhs_part.setZero();
        // vertex 1
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            lhs_part(0, j)  = bary[0]*v0_w0*Tj_times_Hi_[v0_dep0](3*idxA+0, j);
            lhs_part(1, j)  = bary[0]*v0_w0*Tj_times_Hi_[v0_dep0](3*idxA+1, j);
            lhs_part(2, j)  = bary[0]*v0_w0*Tj_times_Hi_[v0_dep0](3*idxA+2, j);

            lhs_part(0, j) += bary[0]*v0_w1*Tj_times_Hi_[v0_dep1](3*idxA+0, j);
            lhs_part(1, j) += bary[0]*v0_w1*Tj_times_Hi_[v0_dep1](3*idxA+1, j);
            lhs_part(2, j) += bary[0]*v0_w1*Tj_times_Hi_[v0_dep1](3*idxA+2, j);

            lhs_part(0, j) += bary[0]*v0_w2*Tj_times_Hi_[v0_dep2](3*idxA+0, j);
            lhs_part(1, j) += bary[0]*v0_w2*Tj_times_Hi_[v0_dep2](3*idxA+1, j);
            lhs_part(2, j) += bary[0]*v0_w2*Tj_times_Hi_[v0_dep2](3*idxA+2, j);

            lhs_part(0, j) += bary[0]*v0_w3*Tj_times_Hi_[v0_dep3](3*idxA+0, j);
            lhs_part(1, j) += bary[0]*v0_w3*Tj_times_Hi_[v0_dep3](3*idxA+1, j);
            lhs_part(2, j) += bary[0]*v0_w3*Tj_times_Hi_[v0_dep3](3*idxA+2, j);

            lhs_part(0, j) += bary[0]*v0_w4*Tj_times_Hi_[v0_dep4](3*idxA+0, j);
            lhs_part(1, j) += bary[0]*v0_w4*Tj_times_Hi_[v0_dep4](3*idxA+1, j);
            lhs_part(2, j) += bary[0]*v0_w4*Tj_times_Hi_[v0_dep4](3*idxA+2, j);

            lhs_part(0, j) += bary[0]*v0_w5*Tj_times_Hi_[v0_dep5](3*idxA+0, j);
            lhs_part(1, j) += bary[0]*v0_w5*Tj_times_Hi_[v0_dep5](3*idxA+1, j);
            lhs_part(2, j) += bary[0]*v0_w5*Tj_times_Hi_[v0_dep5](3*idxA+2, j);

            lhs_part(0, j) += bary[0]*v0_w6*Tj_times_Hi_[v0_dep6](3*idxA+0, j);
            lhs_part(1, j) += bary[0]*v0_w6*Tj_times_Hi_[v0_dep6](3*idxA+1, j);
            lhs_part(2, j) += bary[0]*v0_w6*Tj_times_Hi_[v0_dep6](3*idxA+2, j);

            lhs_part(0, j) += bary[0]*v0_w7*Tj_times_Hi_[v0_dep7](3*idxA+0, j);
            lhs_part(1, j) += bary[0]*v0_w7*Tj_times_Hi_[v0_dep7](3*idxA+1, j);
            lhs_part(2, j) += bary[0]*v0_w7*Tj_times_Hi_[v0_dep7](3*idxA+2, j);
        }
        // vertex 2
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            lhs_part(0, j) += bary[1]*v1_w0*Tj_times_Hi_[v1_dep0](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w0*Tj_times_Hi_[v1_dep0](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w0*Tj_times_Hi_[v1_dep0](3*idxB+2, j);

            lhs_part(0, j) += bary[1]*v1_w1*Tj_times_Hi_[v1_dep1](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w1*Tj_times_Hi_[v1_dep1](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w1*Tj_times_Hi_[v1_dep1](3*idxB+2, j);

            lhs_part(0, j) += bary[1]*v1_w2*Tj_times_Hi_[v1_dep2](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w2*Tj_times_Hi_[v1_dep2](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w2*Tj_times_Hi_[v1_dep2](3*idxB+2, j);

            lhs_part(0, j) += bary[1]*v1_w3*Tj_times_Hi_[v1_dep3](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w3*Tj_times_Hi_[v1_dep3](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w3*Tj_times_Hi_[v1_dep3](3*idxB+2, j);

            lhs_part(0, j) += bary[1]*v1_w4*Tj_times_Hi_[v1_dep4](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w4*Tj_times_Hi_[v1_dep4](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w4*Tj_times_Hi_[v1_dep4](3*idxB+2, j);

            lhs_part(0, j) += bary[1]*v1_w5*Tj_times_Hi_[v1_dep5](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w5*Tj_times_Hi_[v1_dep5](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w5*Tj_times_Hi_[v1_dep5](3*idxB+2, j);

            lhs_part(0, j) += bary[1]*v1_w6*Tj_times_Hi_[v1_dep6](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w6*Tj_times_Hi_[v1_dep6](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w6*Tj_times_Hi_[v1_dep6](3*idxB+2, j);

            lhs_part(0, j) += bary[1]*v1_w7*Tj_times_Hi_[v1_dep7](3*idxB+0, j);
            lhs_part(1, j) += bary[1]*v1_w7*Tj_times_Hi_[v1_dep7](3*idxB+1, j);
            lhs_part(2, j) += bary[1]*v1_w7*Tj_times_Hi_[v1_dep7](3*idxB+2, j);
        }
        // vertex 3
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            lhs_part(0, j) += bary[2]*v2_w0*Tj_times_Hi_[v2_dep0](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w0*Tj_times_Hi_[v2_dep0](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w0*Tj_times_Hi_[v2_dep0](3*idxC+2, j);

            lhs_part(0, j) += bary[2]*v2_w1*Tj_times_Hi_[v2_dep1](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w1*Tj_times_Hi_[v2_dep1](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w1*Tj_times_Hi_[v2_dep1](3*idxC+2, j);

            lhs_part(0, j) += bary[2]*v2_w2*Tj_times_Hi_[v2_dep2](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w2*Tj_times_Hi_[v2_dep2](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w2*Tj_times_Hi_[v2_dep2](3*idxC+2, j);

            lhs_part(0, j) += bary[2]*v2_w3*Tj_times_Hi_[v2_dep3](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w3*Tj_times_Hi_[v2_dep3](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w3*Tj_times_Hi_[v2_dep3](3*idxC+2, j);

            lhs_part(0, j) += bary[2]*v2_w4*Tj_times_Hi_[v2_dep4](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w4*Tj_times_Hi_[v2_dep4](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w4*Tj_times_Hi_[v2_dep4](3*idxC+2, j);

            lhs_part(0, j) += bary[2]*v2_w5*Tj_times_Hi_[v2_dep5](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w5*Tj_times_Hi_[v2_dep5](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w5*Tj_times_Hi_[v2_dep5](3*idxC+2, j);

            lhs_part(0, j) += bary[2]*v2_w6*Tj_times_Hi_[v2_dep6](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w6*Tj_times_Hi_[v2_dep6](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w6*Tj_times_Hi_[v2_dep6](3*idxC+2, j);

            lhs_part(0, j) += bary[2]*v2_w7*Tj_times_Hi_[v2_dep7](3*idxC+0, j);
            lhs_part(1, j) += bary[2]*v2_w7*Tj_times_Hi_[v2_dep7](3*idxC+1, j);
            lhs_part(2, j) += bary[2]*v2_w7*Tj_times_Hi_[v2_dep7](3*idxC+2, j);
        }

        // right hand side
        b(r) = weight_model * ( correspondences[i].on_ps[0] - rhs_part[0] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(0, dcount) * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = weight_model * ( correspondences[i].on_ps[1] - rhs_part[1] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(1, dcount) * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = weight_model * ( correspondences[i].on_ps[2] - rhs_part[2] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(2, dcount) * weight_model) ); // d's
        }
        ++r;
    }
}

//-----------------------------------------------------------------------------


unsigned int
Energy_term_pca_cpc_ps2mesh_skinned::
n_rows_big_sle()
{
    //std::cout << "[INFO] Energy_term_pca_cpc_ps2mesh_skinned::n_rows_big_sle()" << std::endl;

    return (unsigned int)3*energy_term_data_.correspondences.size();
}

//=============================================================================
} // namespace surface_mesh
} // namespace graphene
//=============================================================================
