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


#include "Template_fit.h"

#include <limits>
#include <sstream>
#include <fstream>
#include <iostream>

#include <graphene/surface_mesh/algorithms/surface_mesh_tools/Triangle_kD_tree.h>
#include <graphene/surface_mesh/algorithms/surface_mesh_tools/diffgeo.h>
#include <graphene/surface_mesh/algorithms/surface_mesh_tools/misc.h>
#include <graphene/surface_mesh/algorithms/smoothing/Smoother.h>
#include <graphene/geometry/registration.h>
#include <graphene/geometry/bary_coord.h>
#include <graphene/geometry/Matrix3x3.h>
#include <graphene/utility/Stop_watch.h>

#include "settings.h"
#include "utility/my_helper.h"
#include "utility/Vertex_weighting.h"


//== NAMESPACES ================================================================


namespace graphene {
namespace surface_mesh {


using graphene::geometry::Bounding_box;


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

int Template_fit::DEBUG_ITER_COUNT = 0;

Template_fit::
Template_fit() :
    point_set_(NULL),
    render_callback_(nullptr),
    template_mesh_(NULL),
    character_(NULL),
    fn_folder_fieldliner_(""),
    helper_use_old_laplacian_(false),
    correct_joint_positions_(true),
    fitting_target_api_used_(false)
{
}


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


void
Template_fit::
clear()
{
    // invalidate point_set_ etc.
    point_set_              = NULL;

    // clean-up mesh properties
    clean_up_mesh_properties();
    // invalidate template_mesh_
    template_mesh_ = NULL;

    energy_.clear();

}


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


void
Template_fit::
set_point_set(Point_set* point_set)
{
    point_set_ = point_set;
    energy_.energy_term_data().point_set = point_set;
    fitting_target_api_used_ = false;
}


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


void
Template_fit::
set_template_mesh(Surface_mesh* template_mesh)
{
    template_mesh_ = template_mesh;
    energy_.energy_term_data().template_mesh = template_mesh;
}


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


void
Template_fit::
set_character(graphene::character::Character* character)
{
    character_ = character;
    energy_.energy_term_data().character = character;
}


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


void
Template_fit::
add_fitting_target(const Fitting_target& fitting_target)
{
    fitting_targets_.push_back(fitting_target);
    fitting_target_api_used_ = true;

    // Add new target to energy_term_data
    auto& energy_term_data = energy_.energy_term_data();
    energy_term_data.fitting_targets.push_back(Energy_fitting_target());

    // Copy data from target
    auto& target_data = energy_term_data.fitting_targets.back();
    target_data.point_set = fitting_target.point_set;
    target_data.pose_vector = fitting_target.pose_vector;
    target_data.landmark_property_name = fitting_target.landmark_property_name;
    target_data.landmark_weight_property_name = fitting_target.landmark_weight_property_name;
}


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

void
Template_fit::
setup_energy_fitting_targets()
{

}


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


bool
Template_fit::
load_selection_into_bool_property(const std::string& filename, const std::string& property_name)
{
    std::vector<int> indices;
    if (!graphene::surface_mesh::load_selection_from_file(indices, filename))
    {
        return false;
    }

    auto property = template_mesh_->vertex_property<bool>(property_name, false);
    for (auto idx : indices)
    {
        property[Surface_mesh::Vertex(idx)] = true;
    }

    return true;
}


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


bool
Template_fit::
load_selection_to_omit(const std::string& filename)
{
    std::ifstream ifs(filename.c_str());

    if (ifs.fail())
    {
        std::cerr << "Template_fit::load_selection_to_omit(): [ERROR] Can't open file." << std::endl;
        return false;
    }

    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_selection_to_omit(): [ERROR] No template mesh available." << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<bool> template_points_to_omit;
    template_points_to_omit = template_mesh_->vertex_property<bool>("v:omit", false);
    for (auto v : template_mesh_->vertices())
    {
        template_points_to_omit[v] = false;
    }

    while(!ifs.eof())
    {
        std::string line;
        getline(ifs,line);
        std::stringstream sstr(line);
        size_t idx;
        sstr >> idx;

        if (!ifs)
            break;

        Surface_mesh::Vertex v = Surface_mesh::Vertex(idx);
        template_points_to_omit[v] = true;
    }

    ifs.close();

    return true;
}


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


bool
Template_fit::
load_selection_to_omit(const std::string& filename, const std::string& filename_mapping)
{
    std::ifstream ifs(filename.c_str());

    if (ifs.fail())
    {
        std::cerr << "Template_fit::load_selection_to_omit(): [ERROR] Can't open file." << std::endl;
        return false;
    }

    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_selection_to_omit(): [ERROR] No template mesh available." << std::endl;
        return false;
    }

    // load mapping
    std::vector<int> mapping_vec;
    if (!graphene::surface_mesh::load_selection_from_file(mapping_vec, filename_mapping))
    {
        std::cerr << "[ERROR] While loading mapping." << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<bool> template_points_to_omit;
    template_points_to_omit = template_mesh_->vertex_property<bool>("v:omit", false);
    for (auto v : template_mesh_->vertices())
    {
        template_points_to_omit[v] = false;
    }

    while(!ifs.eof())
    {
        std::string line;
        getline(ifs,line);
        std::stringstream sstr(line);
        size_t idx;
        sstr >> idx;

        if (!ifs)
            break;

        const int map = mapping_vec[ idx ];
        if (map == -1)
        {
            std::cerr << "[ERROR] map == -1" << std::endl;
            return false;
        }
        Surface_mesh::Vertex v = Surface_mesh::Vertex(map);
        template_points_to_omit[v] = true;
    }

    ifs.close();

    return true;
}


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


void
Template_fit::
clear_selection_to_omit()
{
    Surface_mesh::Vertex_property<bool> template_points_to_omit;
    template_points_to_omit = template_mesh_->vertex_property<bool>("v:omit", false);

    for (auto v : template_mesh_->vertices())
    {
        template_points_to_omit[v] = false;
    }
}


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


bool
Template_fit::
load_selection_to_keep(const std::string& filename, bool add)
{
    std::ifstream ifs(filename.c_str());

    if (ifs.fail())
    {
        std::cerr << "Template_fit::load_selection_to_keep(): [ERROR] Can't open file \"" << filename << "\"" << std::endl;
        return false;
    }

    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_selection_to_keep(): [ERROR] No template mesh available." << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<bool> template_points_to_keep;

    if (add)
    {
        // Keep second argument false in order to initialize correctly when no keep property has been set before
        template_points_to_keep = template_mesh_->vertex_property<bool>("v:keep", false);
    }
    else
    {
        template_points_to_keep = template_mesh_->vertex_property<bool>("v:keep", false);
        for (auto v : template_mesh_->vertices())
        {
            template_points_to_keep[v] = false;
        }
    }

    while(!ifs.eof())
    {
        std::string line;
        getline(ifs,line);
        std::stringstream sstr(line);
        size_t idx;
        sstr >> idx;

        if (!ifs)
            break;

        Surface_mesh::Vertex v = Surface_mesh::Vertex(idx);
        template_points_to_keep[v] = true;
    }

    ifs.close();

    return true;
}


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


bool
Template_fit::
load_selection_to_keep(const std::string& filename, const std::string& filename_mapping, const bool add)
{
    std::ifstream ifs(filename.c_str());

    if (ifs.fail())
    {
        std::cerr << "Template_fit::load_selection_to_keep(): [ERROR] Can't open file \"" << filename << "\"" << std::endl;
        return false;
    }

    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_selection_to_keep(): [ERROR] No template mesh available." << std::endl;
        return false;
    }

    // load mapping
    std::vector<int> mapping_vec;
    if (!graphene::surface_mesh::load_selection_from_file(mapping_vec, filename_mapping))
    {
        std::cerr << "Template_fit::load_selection_to_keep(): [ERROR] While loading mapping. Can't open file \"" << filename_mapping << "\"" << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<bool> template_points_to_keep;
    if (add)
    {
        template_points_to_keep = template_mesh_->vertex_property<bool>("v:keep");
    }
    else
    {
        template_points_to_keep = template_mesh_->vertex_property<bool>("v:keep", false);
        for (auto v : template_mesh_->vertices())
        {
            template_points_to_keep[v] = false;
        }
    }

    while(!ifs.eof())
    {
        std::string line;
        getline(ifs,line);
        std::stringstream sstr(line);
        size_t idx;
        sstr >> idx;

        if (!ifs)
            break;

        const int map = mapping_vec[ idx ];
        if (map != -1)
        {
            Surface_mesh::Vertex v = Surface_mesh::Vertex(map);
            template_points_to_keep[v] = true;
        }
    }

    ifs.close();

    return true;
}


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


void
Template_fit::
clear_selection_to_keep()
{
    Surface_mesh::Vertex_property<bool> template_points_to_keep;
    template_points_to_keep = template_mesh_->vertex_property<bool>("v:keep", false);

    for (auto v : template_mesh_->vertices())
    {
        template_points_to_keep[v] = false;
    }
}


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


bool
Template_fit::
load_selection_ground(const std::string& filename)
{
    std::ifstream ifs(filename.c_str());

    if (ifs.fail())
    {
        std::cerr << "Template_fit::load_selection_ground(): [ERROR] Can't open file." << std::endl;
        return false;
    }

    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_selection_ground(): [ERROR] No template mesh available." << std::endl;
        return false;
    }

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > template_points_ground_prop;
    template_points_ground_prop = template_mesh_->mesh_property< std::vector<Surface_mesh::Vertex> >("m:ground_vertices");
    std::vector<Surface_mesh::Vertex>& template_points_ground = template_points_ground_prop[0];

    template_points_ground.clear();

    while(!ifs.eof())
    {
        std::string line;
        getline(ifs,line);
        std::stringstream sstr(line);
        size_t idx;
        sstr >> idx;

        if (!ifs)
            break;

        Surface_mesh::Vertex v = Surface_mesh::Vertex(idx);
        template_points_ground.push_back(v);
    }

    ifs.close();

    return true;
}


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


void
Template_fit::
clear_selection_ground()
{
    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > template_points_ground_prop;
    template_points_ground_prop = template_mesh_->mesh_property< std::vector<Surface_mesh::Vertex> >("m:ground_vertices");

    template_points_ground_prop[0].clear();
}


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


bool
Template_fit::
load_mouth_shut_selection(const std::string &filename)
{
    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_mouth_shut_selection: [ERROR] No template mesh available." << std::endl;
        return false;
    }


    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > mouth_shut_prop
        = template_mesh_->mesh_property< std::vector<Surface_mesh::Vertex> >("m:mouth_shut");
    std::vector<Surface_mesh::Vertex>& mouth_shut  = mouth_shut_prop[0];

    mouth_shut.clear();

    if (!surface_mesh::load_selection_from_file(mouth_shut, filename))
    {
        std::cerr << "[ERROR] Can't open selection for mouth shut on template: " << filename << std::endl;
        return false;
    }

    if (mouth_shut.size() % 2 != 0)
    {
        std::cerr << "[ERROR] Mouth shut size is not divisible by 2."  << std::endl;
        return false;
    }

    return true;
}


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


bool
Template_fit::
load_mouth_shut_selection(const std::string& filename, const std::string& filename_mapping)
{
    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_mouth_shut_selection(): [ERROR] No template mesh available." << std::endl;
        return false;
    }

    // load mapping
    std::vector<int> mapping_vec;
    if (!graphene::surface_mesh::load_selection_from_file(mapping_vec, filename_mapping))
    {
        std::cerr << "Template_fit::load_mouth_shut_selection(): [ERROR] While loading mapping. Can't open file \"" << filename_mapping << "\"" << std::endl;
        return false;
    }

    // load mouth shut selection
    std::vector<Surface_mesh::Vertex> mouth_shut_full;
    if (!graphene::surface_mesh::load_selection_from_file(mouth_shut_full, filename))
    {
        std::cerr << "Template_fit::load_mouth_shut_selection(): [ERROR] Can't open selection for mouth shut on head template. File \"" << filename << "\"" << std::endl;
        return false;
    }

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > mouth_shut_prop
        = template_mesh_->mesh_property< std::vector<Surface_mesh::Vertex> >("m:mouth_shut");
    std::vector<Surface_mesh::Vertex>& mouth_shut  = mouth_shut_prop[0];
    mouth_shut.resize(mouth_shut_full.size());
    for (size_t i = 0; i < mouth_shut_full.size(); ++i)
    {
        Surface_mesh::Vertex& full_v = mouth_shut_full[i];

        if (mapping_vec[full_v.idx()] == -1)
        {
            std::cerr << "Template_fit::load_mouth_shut_selection(): [ERROR] mapping_vec[full_v.idx()] == -1" << std::endl;
            return false;
        }

        mouth_shut[i] = Surface_mesh::Vertex(mapping_vec[full_v.idx()]);
    }

    if (mouth_shut.size() % 2 != 0)
    {
        std::cerr << "[ERROR] Mouth shut size is not divisible by 2."  << std::endl;
        return false;
    }

    return true;
}


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


bool
Template_fit::
load_fieldlines(const std::string& filename)
{
    std::ifstream ifs(filename.c_str());
    if (ifs.fail())
    {
        std::cerr << "Template_fit::load_fieldlines(): [ERROR] Can't open file \"" << filename << "\"" << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<bool> template_points_to_omit
        = template_mesh_->get_vertex_property<bool>("v:omit");

    auto template_points = template_mesh_->vertex_property<Point>("v:point");

    energy_.energy_term_data().fieldlines.clear();

    const std::string ext = filename.substr(filename.rfind('.') + 1);
    if (ext == "fl")
    {
        while(!ifs.eof())
        {
            std::string line;
            getline(ifs,line);
            std::stringstream sstr(line);

            size_t idx;
            sstr >> idx;
            Surface_mesh::Vertex v = Surface_mesh::Vertex(idx);

            double x, y, z;
            sstr >> x;
            sstr >> y;
            sstr >> z;
            Point p(x, y, z);

            if (!ifs)
                break;

            Correspondence corr;
            corr.on_template          = Point(0);
            corr.on_template_n        = Normal(0);
            corr.on_template_vh       = v;
            corr.on_template_bc       = Vec3f(0);
            corr.on_ps                = p;
            corr.on_ps_n              = Vec3f(0);
            corr.weight               = 1.0;
            corr.distance_point2point = distance( template_mesh_->position(v), p );
            corr.constr_dir           = corresp_dir_mesh2ps;

            if (template_points_to_omit)
            {
                if (!template_points_to_omit[v])
                {
                    energy_.energy_term_data().fieldlines.push_back(corr);
                }
            }
            else
            {
                energy_.energy_term_data().fieldlines.push_back(corr);
            }
        }
    }
    else if (ext == "trigfl")
    {
        while(!ifs.eof())
        {
            std::string line;
            getline(ifs,line);
            std::stringstream sstr(line);

            size_t idx;
            sstr >> idx;
            Surface_mesh::Face f = Surface_mesh::Face(idx);

            double a, b;
            sstr >> a;
            sstr >> b;
            double c = 1.0-a-b;
            if (c < 0)
            {
                double sum = a+b;
                a /= sum;
                b /= sum;
                c = 1.0-a-b;
            }

            double x, y, z;
            sstr >> x;
            sstr >> y;
            sstr >> z;
            Point p(x, y, z);

            if (!ifs)
                break;

            // get triangle vertices
            auto fvit = template_mesh_->vertices(f);
            const auto v0 = *fvit;
            const auto v1 = *(++fvit);
            const auto v2 = *(++fvit);

            Correspondence corr;
            corr.face                 = f;
            corr.on_ps                = p;
            corr.on_ps_n              = Vec3f(0);
            corr.on_template          = a*template_points[v0] + b*template_points[v1] + c*template_points[v2];
            corr.on_template_bc       = Vec3f(a, b, 1.0-a-b);
            corr.distance_point2point = distance( corr.on_template, p );
            corr.constr_dir           = corresp_dir_ps2mesh;
            corr.on_template_n        = Normal(0);
            corr.weight               = 1.0;

            if (template_points_to_omit)
            {
                if (!(template_points_to_omit[v0] ||
                      template_points_to_omit[v1] ||
                      template_points_to_omit[v2]))
                {
                    energy_.energy_term_data().fieldlines.push_back(corr);
                }
            }
            else
            {
                energy_.energy_term_data().fieldlines.push_back(corr);
            }
        }
    }

    ifs.close();

    return true;
}


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


bool
Template_fit::
update_local_weights_flcs()
{
    double dist_min = DBL_MAX;
    double dist_max = DBL_MIN;
    for (unsigned int i = 0; i < energy_.energy_term_data().fieldlines.size(); ++i)
    {
        const Correspondence& corr = energy_.energy_term_data().fieldlines[i];

        dist_min = std::min(dist_min, corr.distance_point2point);
        dist_max = std::max(dist_max, corr.distance_point2point);
    }

    if (dist_min >= dist_max)
    {
        std::cerr << "[ERROR] dist_min >= dist_max" << std::endl;
        exit(1);
    }

    double min_weight =   1.0;
    double max_weight = 100.0;
    for (unsigned int i = 0; i < energy_.energy_term_data().fieldlines.size(); ++i)
    {
        Correspondence& corr = energy_.energy_term_data().fieldlines[i];
        double s = (corr.distance_point2point - dist_min)/(dist_max - dist_min); // in [0,1]

        s = pow(s, 10);

        corr.weight = min_weight + s*(max_weight - min_weight);
    }

    return true;
}


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


bool
Template_fit::
load_bounding_mesh(const std::string& filename)
{
    return bounding_mesh_.read(filename);
}


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


bool Template_fit::load_ear_landmarks_template(const std::string &filename)
{
    if (template_mesh_ == nullptr)
    {
        std::cerr << "Template_fit::load_ear_landmarks_template: [ERROR] No template mesh available." << std::endl;
        return false;
    }

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > mears
            = template_mesh_->mesh_property< std::vector<Surface_mesh::Vertex> >("m:ears_lm");

    std::vector<Surface_mesh::Vertex>& mears_vec = mears[0];

    return load_selection_from_file(mears_vec, filename);
}

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

bool
Template_fit::
load_ear_landmarks_template(const std::string& filename, const std::string& filename_mapping)
{
    if (template_mesh_ == nullptr)
    {
        std::cerr << "Template_fit::load_ear_landmarks_template: [ERROR] No template mesh available." << std::endl;
        return false;
    }

    // load mapping
    std::vector<int> mapping_vec;
    if (!graphene::surface_mesh::load_selection_from_file(mapping_vec, filename_mapping))
    {
        std::cerr << "Template_fit::load_ear_landmarks_template(): [ERROR] While loading mapping. Can't open file \"" << filename_mapping << "\"" << std::endl;
        return false;
    }

    // load ear landmarks selection
    std::vector<Surface_mesh::Vertex> ear_landmarks_full;
    if (!graphene::surface_mesh::load_selection_from_file(ear_landmarks_full, filename))
    {
        std::cerr << "Template_fit::load_ear_landmarks_template(): [ERROR] Can't open selection for ear landmarks. File \"" << filename << "\"" << std::endl;
        return false;
    }

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > mears
            = template_mesh_->mesh_property< std::vector<Surface_mesh::Vertex> >("m:ears_lm");
    std::vector<Surface_mesh::Vertex>& mears_vec = mears[0];
    mears_vec.resize(ear_landmarks_full.size());
    for (size_t i = 0; i < ear_landmarks_full.size(); ++i)
    {
        Surface_mesh::Vertex& full_v = ear_landmarks_full[i];

        if (mapping_vec[full_v.idx()] == -1)
        {
            std::cerr << "Template_fit::load_ear_landmarks_template(): [ERROR] mapping_vec[full_v.idx()] == -1" << std::endl;
            return false;
        }

        mears_vec[i] = Surface_mesh::Vertex(mapping_vec[full_v.idx()]);
    }

    return true;
}

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

bool Template_fit::load_ear_landmarks_pointset(const std::string &filename)
{
    if (point_set_ == nullptr)
    {
        std::cerr << "Template_fit::load_ear_landmarks_pointset: [ERROR] No pointset available." << std::endl;
        return false;
    }

    return load_selection_from_file(point_set_->lm_ears_, filename);
}

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


bool
Template_fit::
load_vertexweighting(const std::string &filename, const std::string& property_name, bool multiply)
{
    return graphene::surface_mesh::load_vertexweighting(template_mesh_, filename, property_name, multiply);
}


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


bool
Template_fit::
load_vertexweighting(const std::string& filename, const std::string& filename_mapping, const std::string& property_name, bool multiply)
{
    return graphene::surface_mesh::load_vertexweighting(template_mesh_, filename, filename_mapping, property_name, multiply);
}


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


bool Template_fit::clear_vertexweighting(const std::string &property_name)
{
    return graphene::surface_mesh::clear_vertexweighting(template_mesh_, property_name);
}


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


bool
Template_fit::
do_translation_transformation_lm(const Transformation_direction trans_dir, const std::string& lm_prop_name, const std::string& lm_weight_prop_name)
{
    if (point_set_ == NULL || template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::do_similarity_transformation_lm(...): [ERROR] Either no template or no point set present. Aborting..." << std::endl;
        return false;
    }

    Landmarks_manager& landmarks_manager = energy_.energy_term_data().landmarks_manager;

    if (! landmarks_manager.check_dimension(point_set_, template_mesh_, lm_prop_name))
    {
        std::cerr << "Template_fit::do_similarity_transformation_lm(...): [ERROR] Landmarks have different dimensions. Aborting..." << std::endl;
        return false;
    }

    if (trans_dir == trans_dir_mesh2ps)
    {
        std::vector<Point> lm_ps;
        std::vector<Point> lm_templ;
        landmarks_manager.get_landmarks_combined_stdvec(template_mesh_, point_set_, lm_templ, lm_ps, lm_prop_name, lm_weight_prop_name);
        // landmarks_manager.get_landmarks_template_stdvec(template_mesh_, lm_templ);
        size_t num_landmarks = lm_templ.size();

        Point avg_dist(0.0f);
        for (size_t landmark_index = 0; landmark_index < num_landmarks; ++landmark_index)
        {
            avg_dist += lm_ps[landmark_index] - lm_templ[landmark_index];
        }
        avg_dist /= num_landmarks;

        const Mat4f M = Mat4f::translate(avg_dist);

        // transform mesh
        transform_mesh(*template_mesh_, M);
        update_restpose();
    }
    else if (trans_dir == trans_dir_ps2mesh)
    {
        std::vector<Point> lm_ps;
        std::vector<Point> lm_templ;
        landmarks_manager.get_landmarks_combined_stdvec(template_mesh_, point_set_, lm_templ, lm_ps, lm_prop_name, lm_weight_prop_name);
        // landmarks_manager.get_landmarks_point_set_stdvec(point_set_, lm_ps);
        // landmarks_manager.get_landmarks_template_stdvec(template_mesh_, lm_templ);
        size_t num_landmarks = lm_templ.size();

        Point avg_dist(0.0f);
        for (size_t landmark_index = 0; landmark_index < num_landmarks; ++landmark_index)
        {
            avg_dist += lm_templ[landmark_index] - lm_ps[landmark_index];
        }
        avg_dist /= num_landmarks;

        const Mat4f M = Mat4f::translate(avg_dist);

        // transform points
        transform_points(M);
    }

    return true;
}


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


bool
Template_fit::
do_similarity_transformation_lm(const Transformation_direction trans_dir, const std::string& lm_prop_name, const std::string& lm_weight_prop_name)
{
    if (point_set_ == NULL || template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::do_similarity_transformation_lm(...): [ERROR] Either no template or no point set present. Aborting..." << std::endl;
        return false;
    }

    Landmarks_manager& landmarks_manager = energy_.energy_term_data().landmarks_manager;

    if (! landmarks_manager.check_dimension(point_set_, template_mesh_, lm_prop_name))
    {
        std::cerr << "Template_fit::do_similarity_transformation_lm(...): [ERROR] Landmarks have different dimensions. Aborting..." << std::endl;
        return false;
    }

    if (trans_dir == trans_dir_mesh2ps)
    {
        std::vector<Point> lm_ps;
        std::vector<Point> lm_templ;
        landmarks_manager.get_landmarks_combined_stdvec(template_mesh_, point_set_, lm_templ, lm_ps, lm_prop_name, lm_weight_prop_name);
        // landmarks_manager.get_landmarks_template_stdvec(template_mesh_, lm_templ);

        const Mat4f M = geometry::registration(lm_templ,
                                               lm_ps,
                                               graphene::geometry::CONFORMAL_REGISTRATION);

        // transform mesh
        transform_mesh(*template_mesh_, M);
        update_restpose();
    }
    else if (trans_dir == trans_dir_ps2mesh)
    {
        std::vector<Point> lm_ps;
        std::vector<Point> lm_templ;
        landmarks_manager.get_landmarks_combined_stdvec(template_mesh_, point_set_, lm_templ, lm_ps, lm_prop_name, lm_weight_prop_name);
        // landmarks_manager.get_landmarks_point_set_stdvec(point_set_, lm_ps);
        // landmarks_manager.get_landmarks_template_stdvec(template_mesh_, lm_templ);

        const Mat4f M = geometry::registration(lm_ps,
                                               lm_templ,
                                               graphene::geometry::CONFORMAL_REGISTRATION);

        // transform points
        transform_points(M);
    }

    return true;
}


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


bool
Template_fit::
do_similarity_transformation_cpc(const Transformation_direction trans_dir,
                                 const Correspondences_settings& corresp_settings,
                                 double& relative_change)
{
    if (point_set_ == NULL || template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::do_similarity_transformation_cpc(...): [ERROR] Either no template or no point set present. Aborting..." << std::endl;

        relative_change = 0.0;
        return false;
    }

    if (trans_dir == trans_dir_ps2mesh)
    {
        std::vector<Correspondence> corr;
        if (!compute_correspondences(template_mesh_, point_set_, corresp_settings, corr))
        {
            std::cerr << "Template_fit::do_similarity_transformation_cpc(...): [ERROR] Can't compute correspondences. Aborting..." << std::endl;

            relative_change = 0.0;
            return false;
        }

        std::vector<Point> points_ps;
        std::vector<Point> points_template;
        for (unsigned int i = 0; i < corr.size(); ++i)
        {
            points_ps.push_back(corr[i].on_ps);
            points_template.push_back(corr[i].on_template);
        }

        double temp_relative_change;
        const Mat4f M = geometry::registration(points_ps,
                                               points_template,
                                               graphene::geometry::CONFORMAL_REGISTRATION,
                                               &temp_relative_change);

        // transform points
        transform_points(M);

        relative_change = temp_relative_change;
        return true;
    }
    else
    {
        std::cerr << "[ERROR] in Template_fit::do_similarity_transformation_cpc()" << std::endl;
        return false;
    }

    return true;
}


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

void
Template_fit::
start(Correspondences_settings &corresp_settings,
      const unsigned int max_iter,
      const double conv_val,
      const double until_lambda,
      const bool make_plastic,
      const bool no_decrease,
      const bool use_dualquaternion_skinning)
{
    // If we did not use the add_fitting_target API, we expect the point_set_ variable to be set
    if (template_mesh_ == NULL || (point_set_ == NULL && fitting_targets_.size() == 0))
    {
        std::cerr << "Template_fit::start(): [ERROR] Either no template or no point set present. Aborting..." << std::endl;
        return;
    }

    update_restpose();

    //
    // TODO(swenninger):
    //   We could (and should) remove 'fitting_target_api_used_'
    //   at some point and always work with the set of 'fitting_targets_'
    //   added to the 'Template_fit'. This would unify a lot of code
    //   in this function
    //

    bool       must_compute_cpc_corresp = false;
    bool     need_skinning_mesh_for_cpc = false;
    bool       must_compute_flc_corresp = false;
    bool         must_handle_collisions = false;
    double       lambda                 = 0.0;
    double       error_total_previous   = DBL_MAX;
    double       error_total_current    = 0.0;
    double       conv_val_rel           = 0.0;
    unsigned int iter_first_loop        = 0;
    unsigned int iter_second_loop       = 0;
    bool         first_loop_done        = false;
    bool         second_loop_done       = false;

    graphene::character::Skinning_mode skinning_mode =
        use_dualquaternion_skinning ?
        graphene::character::DUALQUAT_BLENDING :
        graphene::character::LINEAR_BLENDING;

    //TODO Do we need to check valid settings here?!
    //check_if_settings_are_valid( corresp_settings, weights_settings, robustness_settings );

    energy_.energy_term_list().update_and_init_active();

    if (energy_.energy_term_list().active_terms().empty())
    {
        std::cerr << "Template_fit::start(): [ERROR] No active energy terms. Aborting..." << std::endl;
        return;
    }

    std::cout << "Template_fit::start: [STATUS] Active energy terms: \n";
    const std::vector<Energy_term*> &active_terms = energy_.energy_term_list().active_terms();
    for (size_t i=0; i < active_terms.size(); ++i)
    {
        const Energy_term* t = active_terms[i];
        std::cout << "  -" << t->get_name() << std::endl;
    }

    must_compute_cpc_corresp = energy_.energy_term_list().need_correspondences();
    need_skinning_mesh_for_cpc = energy_.energy_term_list().need_skinning_mesh_for_computing_correspondences();
    if (need_skinning_mesh_for_cpc && !character_)
    {
        std::cerr << "Template_fit::start(): [ERROR] need_skinning_mesh_for_cpc && !character_. Exit!" << std::endl;
        exit(1);
    }

    if (fitting_targets_.size() > 0)
    {
        setup_energy_fitting_targets();
    }

    auto& fitting_targets_data = energy_.energy_term_data().fitting_targets;
    size_t num_fitting_targets  = fitting_targets_data.size();

    must_compute_flc_corresp = energy_.energy_term_list().need_fieldlines();
    must_handle_collisions   = bounding_mesh_.n_vertices() ? true : false;

    // vertex positions before deformation
    auto mesh_points_before_deformation = template_mesh_->vertex_property<Point>("v:point_before_def");

    // state if vertices collide
    auto mesh_vertices_collision_weight_v_vs_tri = template_mesh_->vertex_property<double>("v:collision_weight_v_vs_tri");
    auto mesh_vertices_collision_weight_tri_vs_p = template_mesh_->vertex_property<double>("v:collision_weight_tri_vs_p");

    unsigned int min_n_colls    = std::numeric_limits<int>::max();
    int       n_hit_min_n_colls = 0;
    const int n_hits_allowed    = 3;

    // int dummy = 0;
    while (!first_loop_done)
    {
        iter_first_loop++;
        //std::cout << "1st loop: " << iter_first_loop << std::endl;

        // backup previous positions for collision handling
        if (must_handle_collisions)
        {
            for (auto v : template_mesh_->vertices())
            {
                mesh_points_before_deformation[v] = template_mesh_->position(v);
            }

            // clear collisions since we start from an non-collided state
            energy_.energy_term_data().collisions_v_vs_tri.clear();
            energy_.energy_term_data().collisions_tri_vs_p.clear();

            for (auto v : template_mesh_->vertices())
            {
                mesh_vertices_collision_weight_v_vs_tri[v] = 0.0;
                mesh_vertices_collision_weight_tri_vs_p[v] = 0.0;
            }
        }

        // FLC
        if (must_compute_flc_corresp)
        {
            // compute fieldlines
            const std::string fn_fieldliner_mesh("/tmp/fieldliner_mesh.off");
            template_mesh_->write(fn_fieldliner_mesh);
            const std::string fn_fieldlines("/tmp/fieldlines.fl");  // for skin_to_skel or skel_to_skin
            // const std::string fn_fieldlines("/tmp/fieldlines.trigfl"); // for skel_to_skin
            std::stringstream command_fieldliner;
            command_fieldliner << "cd " << fn_folder_fieldliner_ << " ; "
                               << fn_folder_fieldliner_ << "skinning" << " skin_to_skel " << fn_fieldliner_mesh << " " << fn_fieldlines << " " << "1.0" << " " << "0" << " " << "1"; // percentage & premium & symmetry
                            // << fn_folder_fieldliner_ << "skinning" << " skel_to_skin " << fn_fieldliner_mesh << " " << fn_fieldlines << " " << "1.0"; // dist_scale
            int fieldliner_result = system( command_fieldliner.str().c_str() );

            if (fieldliner_result != 0)
                std::cout << "[WARNING] Fieldliner returned non-zero value: " << fieldliner_result << std::endl;

            // load just computed fieldlines
            if (!load_fieldlines(fn_fieldlines))
            {
                std::cerr << "[ERROR] - !load_fieldlines(...)" << std::endl;
                exit(1);
            }

            // update local weights for FLCs
            // note: not used anymore
            // update_local_weights_flcs();
        }

        iter_second_loop  = 0;
        second_loop_done  = false;
        min_n_colls       = INT_MAX;
        n_hit_min_n_colls = 0;
        while (!second_loop_done)
        {
            iter_second_loop++;
            //std::cout << "2nd loop: " << iter_second_loop << std::endl;

            // CPC
            if (must_compute_cpc_corresp)
            {
                //std::cout << "Template_fit::start(): [INFO] Compute correspondences." << std::endl;
                if (need_skinning_mesh_for_cpc)
                {
                    if (fitting_target_api_used_)
                    {
                        // backup data
                        const std::vector<Point> points_bak = template_mesh_->get_vertex_property<Point>("v:point").vector();
                        std::vector<Normal> v_normals_bak;
                        auto v_normals_prop = template_mesh_->get_vertex_property<Normal>("v:normal");
                        if (v_normals_prop)
                            v_normals_bak = v_normals_prop.vector();
                        std::vector<Normal> h_normals_bak;
                        auto h_normals_prop = template_mesh_->get_halfedge_property<Normal>("h:normal");
                        if (h_normals_prop)
                            h_normals_bak = h_normals_prop.vector();
                        std::vector<Mat4f> pose_vector_bak;
                        surface_mesh::get_pose_vector(character_->skeleton(), pose_vector_bak);

                        for (size_t fitting_target_index = 0;
                             fitting_target_index < num_fitting_targets;
                             ++fitting_target_index)
                        {
                            auto& fitting_target = fitting_targets_[fitting_target_index];
                            auto& target_data = fitting_targets_data[fitting_target_index];

                            // If the target specifies its own vertexweight property, use it when
                            // weighting correspondences
                            if (!fitting_target.vertexweighting_property_name.empty())
                            {
                                corresp_settings.vertex_weight_property_name = fitting_target.vertexweighting_property_name;
                            }
                            else
                            {
                                // Use default vertex weighting
                                corresp_settings.vertex_weight_property_name = "v:fitting_weight";
                            }

                            // from TPose to ScanPose
                            surface_mesh::set_pose_vector(*target_data.pose_vector, character_->skeleton());
                            character_->apply_skinning_on_CPU(true, skinning_mode); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals

                            if (!compute_correspondences(template_mesh_, target_data.point_set, corresp_settings, target_data.correspondences))
                            {
                                return;
                            }
                        }

                        sum_correspondences();

                        // restore
                        template_mesh_->get_vertex_property<Point>("v:point").vector() = points_bak;
                        if (v_normals_prop)
                            v_normals_prop.vector() = v_normals_bak;
                        if (h_normals_prop)
                            h_normals_prop.vector() = h_normals_bak;
                        surface_mesh::set_pose_vector(pose_vector_bak, character_->skeleton());
                    }
                    else
                    {
                        // backup data
                        const std::vector<Point> points_bak = template_mesh_->get_vertex_property<Point>("v:point").vector();
                        std::vector<Normal> v_normals_bak;
                        auto v_normals_prop = template_mesh_->get_vertex_property<Normal>("v:normal");
                        if (v_normals_prop)
                            v_normals_bak = v_normals_prop.vector();
                        std::vector<Normal> h_normals_bak;
                        auto h_normals_prop = template_mesh_->get_halfedge_property<Normal>("h:normal");
                        if (h_normals_prop)
                            h_normals_bak = h_normals_prop.vector();

                        // from TPose to ScanPose
                        character_->apply_skinning_on_CPU(true, skinning_mode); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals

                        if (!compute_correspondences(template_mesh_, point_set_, corresp_settings, energy_.energy_term_data().correspondences))
                        {
                            return;
                        }
                        sum_correspondences(energy_.energy_term_data().correspondences);

                        if (render_callback_)
                            render_callback_("Correspondences");

                        // restore
                        template_mesh_->get_vertex_property<Point>("v:point").vector() = points_bak;
                        if (v_normals_prop)
                            v_normals_prop.vector() = v_normals_bak;
                        if (h_normals_prop)
                            h_normals_prop.vector() = h_normals_bak;
                    }
                }
                else
                {
                    if (!compute_correspondences(template_mesh_, point_set_, corresp_settings, energy_.energy_term_data().correspondences))
                    {
                        return;
                    }
                    sum_correspondences(energy_.energy_term_data().correspondences);
                }
            }

            // ------------------------------------------------------------------------------------------- minimize E_total

            // if minimize returns false, a severe error occured -> abort
            if (! energy_.minimize( conv_val )) // this deforms the template mesh
            {
                std::cerr << "Template_fit::start(): [ERROR] Minimize failed -> Aborting..." << std::endl;
                return;
            }

            if (need_skinning_mesh_for_cpc)
            {
                character_->on_shape_changed_shape_in_tpose_but_skeleton_posed(correct_joint_positions_); // true: with correcting joint positions turned on

                //DEBUG
                if (false)
                {
                    std::stringstream ss;
                    ss << std::setfill('0') << std::setw(3) << DEBUG_ITER_COUNT << "_template_fit.bim";
                    character_->write_bim(ss.str().c_str());
                    DEBUG_ITER_COUNT++;
                }
            }

            if (render_callback_)
                render_callback_("Template_fit: step");

            // ------------------------------------------------------------------------------------------- check termination criteria
            error_total_current = energy_.get_function_value();

            conv_val_rel = std::abs(error_total_current - error_total_previous) / error_total_previous;

            if (conv_val_rel < conv_val)
            {
                second_loop_done = true;
            }
            error_total_previous = error_total_current;

            if (iter_second_loop >= max_iter)
            {
                second_loop_done = true;
                first_loop_done = true;
            }
            // ------------------------------------------------------------------------------------------- soft collision handling
            if (must_handle_collisions)
            {
                const unsigned int n_colls_v_vs_tri = update_collision_correspondences_v_vs_tri();
                const unsigned int n_colls_tri_vs_p = update_collision_correspondences_tri_vs_p();
                const unsigned int n_colls = n_colls_v_vs_tri; // + n_colls_tri_vs_p;

                if (n_colls == min_n_colls)
                {
                    n_hit_min_n_colls++;
                }
                unsigned int min_n_colls_before = min_n_colls;
                min_n_colls = std::min(min_n_colls, n_colls);
                if (min_n_colls < min_n_colls_before)
                {
                    n_hit_min_n_colls = 0;
                }
                if (n_colls > 0)
                {
                    if (n_hit_min_n_colls <= n_hits_allowed)
                    {
                        second_loop_done = false;
                    }
                    else
                    {
                        if (conv_val_rel < conv_val)
                        {
                            second_loop_done = true;
                        }
                    }
                }
            }
        }

        if (must_handle_collisions)
        {
            // write mesh before hard collision resolve
            // std::ostringstream streamObj;
            // streamObj << energy_.energy_term_list().get_termination_lambda();
            // template_mesh_->write("hard_before_intermed_mesh_" + streamObj.str() + ".off");
        }

        // write intermediate results
        // {
        //     std::ostringstream streamObj;
        //     streamObj << energy_.energy_term_list().get_termination_lambda();
        //     template_mesh_->write("intermediate_result_" + streamObj.str() + ".off");
        // }

        // ---------------------------------------------------------------------------------------- allow plastic deformation
        if (make_plastic)
        {
            update_restpose();
        }

        // hard collision resolve
        if (must_handle_collisions && n_hit_min_n_colls > n_hits_allowed)
        {
            // both dir: tri_vs_p before AND v_vs_tri afterwards
            // collision_resolve_tri_vs_p();
            collision_resolve_v_vs_tri();

            // write mesh after hard collision resolve
            // std::ostringstream streamObj;
            // streamObj << energy_.energy_term_list().get_termination_lambda();
            // template_mesh_->write("hard_after_intermed_mesh_" + streamObj.str() + ".off");
        }

        energy_.energy_term_list().apply_weight_multipliers();
        lambda = energy_.energy_term_list().get_termination_lambda();

        // check termination criteria
        if (lambda < until_lambda || no_decrease)
        {
            first_loop_done = true;
            std::cout << "Template_fit::start: [STATUS] Termination criterion reached." << std::endl;
        }
        else
        {
            const Energy_term* E_lambda = energy_.energy_term_list().get_term_for_termination_lambda();

            std::cout << "Template_fit::start: [STATUS] "
                      << "Iteration " << iter_first_loop << ". "
                      << "Termination criterion: "
                      <<  lambda << " < " << until_lambda
                       << " of "
                       << "\"" << E_lambda->get_name() << "\"."
                       << std::endl;
        }
    }

    // remove property of vertex positions before deformation
    template_mesh_->remove_vertex_property(mesh_points_before_deformation);
    template_mesh_->remove_vertex_property(mesh_vertices_collision_weight_v_vs_tri);
    template_mesh_->remove_vertex_property(mesh_vertices_collision_weight_tri_vs_p);
}


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


void
Template_fit::
write_mesh_backtransformed(const std::string &filename_mesh )
{
    Surface_mesh tmp_mesh = *template_mesh_;
    auto tmp_mesh_points  = tmp_mesh.vertex_property<Point>("v:point");

    Mat4f inv_trans = Mat4f::identity();
    if (template_mesh_ && point_set_)
    {
        inv_trans = inverse(point_set_->temp_transformation_);
    }

    for (auto v : tmp_mesh.vertices())
    {
        Point p = tmp_mesh_points[v];
        p = affine_transform(inv_trans, p);
        tmp_mesh_points[v] = p;
    }
    tmp_mesh.update_face_normals();
    tmp_mesh.update_vertex_normals();
    tmp_mesh.write(filename_mesh);
}


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


double
Template_fit::
evaluate_fitting()
{
    return energy_.evaluate_fitting();
}


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


void
Template_fit::
sum_correspondences(const std::vector<Correspondence>& correspondences)
{
    double& sum_corresp_weights = energy_.energy_term_data().sum_corresp_weights;
    sum_corresp_weights = 0.0;
    for (auto c : correspondences)
    {
        sum_corresp_weights += c.weight;
    }
}


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


void
Template_fit::
sum_correspondences()
{
    double& sum_corresp_weights = energy_.energy_term_data().sum_corresp_weights;
    sum_corresp_weights = 0.0;

    for (size_t i = 0; i < fitting_targets_.size(); ++i)
    {
        const auto& target = energy_.energy_term_data().fitting_targets[i];
        for (auto c : target.correspondences)
        {
            sum_corresp_weights += c.weight;
        }
    }
}


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


void
Template_fit::
update_restpose()
{
#ifdef BE_VERBOSE
    std::cerr << "in: 'update_restpose(...)'" << std::endl;
#endif

    // update normals
    template_mesh_->update_face_normals();
    template_mesh_->update_vertex_normals();

    // clean-up mesh properties
    clean_up_mesh_properties();

    //auto template_points = template_mesh_->vertex_property<Point>("v:point");
    auto template_points = template_mesh_->vertex_property<Point>("v:unskinned_point");


    // update respose position
    auto template_restpose_points = template_mesh_->vertex_property<Point>("v:rest_pose");
    template_restpose_points.vector() = template_points.vector();


    // update properties
    update_area_property();
    update_cotan_property();
    update_laplaciancoordinate_property();
    update_aniso_laplace_properties();

    // update rotations
    // auto edge_rotation = template_mesh_->edge_property<Mat4f>("e:edge_rotation");
    // for (auto e : template_mesh_->edges())
    // {
    //     edge_rotation[e] = Mat4f::identity();
    // }
    auto localrotPOS  = template_mesh_->vertex_property<Mat4f>("v:localrotationPOS");
    for (auto v : template_mesh_->vertices())
    {
        localrotPOS[v] = Mat4f::identity();
    }
    // update RHS_R{i,e}_xold
    auto template_aniso_laplace = template_mesh_->edge_property<Point>("e:aniso_laplace");
    auto edge_rotation          = template_mesh_->edge_property<Point>("e:edge_rotation");
    for (auto e : template_mesh_->edges())
    {
        edge_rotation[e] = template_aniso_laplace[e];
    }
    auto template_laplacian_coordinates = template_mesh_->vertex_property<Point>("v:laplacian_coordinates");
    auto localrot  = template_mesh_->vertex_property<Point>("v:localrotation");
    for (auto v : template_mesh_->vertices())
    {
        localrot[v] = template_laplacian_coordinates[v];
    }
}


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


void
Template_fit::
transform_points(const Mat4f& M)
{
#ifdef BE_VERBOSE
    std::cerr << "in: 'transform_points(...)'" << std::endl;
#endif

    if (!point_set_)
    {
        std::cerr << "Template_fit::transform_points(...): [ERROR] No point set available. Aborting..." << std::endl;
        return;
    }

    // update total transformation of point-set
    Mat4f& trans = point_set_->temp_transformation_;
    trans = M * trans;

    // normal matrix
    Mat3f normal_matrix = transpose(inverse(Mat3f(M)));

    // transform full original pointset if available
    for (unsigned int i = 0; i < point_set_->points_.size(); ++i)
    {
        Point p = point_set_->points_[i];
        p = affine_transform(M, p);
        point_set_->points_[i] = p;

        Point n = point_set_->normals_[i];
        n = normal_matrix * n;
        n.normalize();
        point_set_->normals_[i] = n;
    }
}


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


void
Template_fit::
clean_up_mesh_properties()
{
    if (template_mesh_ == NULL)
        return;

    // clean-up mesh properties
    auto template_restpose_points = template_mesh_->get_vertex_property<Point>("v:rest_pose");
    template_mesh_->remove_vertex_property(template_restpose_points);
    auto area_V               = template_mesh_->get_vertex_property<double>("v:area");
    template_mesh_->remove_vertex_property(area_V);
    auto cotan_V              = template_mesh_->get_edge_property<double>("e:cotan");
    template_mesh_->remove_edge_property(cotan_V);
    auto area_E               = template_mesh_->get_edge_property<double>("fitting:area");
    template_mesh_->remove_edge_property(area_E);
    auto cotan_E              = template_mesh_->get_halfedge_property<double>("fitting:cotan");
    template_mesh_->remove_halfedge_property(cotan_E);
    auto inner_edges_prop     = template_mesh_->get_mesh_property< std::vector< Surface_mesh::Edge > >("inner_edges");
    template_mesh_->remove_mesh_property(inner_edges_prop);
//    auto edge_rotation        = template_mesh_->edge_property<Mat4f>("e:edge_rotation");
    auto edge_rotation        = template_mesh_->get_edge_property<Point>("e:edge_rotation");
    template_mesh_->remove_edge_property(edge_rotation);
    auto localrotPOS          = template_mesh_->get_vertex_property<Mat4f>("v:localrotationPOS");
    template_mesh_->remove_vertex_property(localrotPOS);
    auto localrot             = template_mesh_->get_vertex_property<Point>("v:localrotation");
    template_mesh_->remove_vertex_property(localrot);
    auto template_laplacian_coordinates = template_mesh_->get_vertex_property<Point>("v:laplacian_coordinates");
    template_mesh_->remove_vertex_property(template_laplacian_coordinates);
    auto template_aniso_laplace = template_mesh_->get_edge_property<Point>("e:aniso_laplace");
    template_mesh_->remove_edge_property(template_aniso_laplace);
}


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


unsigned int
Template_fit::
update_collision_correspondences_v_vs_tri()
{
    if (bounding_mesh_.n_vertices() == 0)
    {
        std::cerr << "[ERROR] update_collision_correspondences_v_vs_tri() - bounding_mesh_.n_vertices() == 0" << std::endl;
        exit(1);
        return std::numeric_limits<int>::max();
    }

    auto mesh_points_before_deformation = template_mesh_->get_vertex_property<Point>("v:point_before_def");
    if (!mesh_points_before_deformation)
    {
        std::cerr << "[ERROR] update_collision_correspondences_v_vs_tri() - !mesh_points_before_deformation" << std::endl;
        exit(1);
        return std::numeric_limits<int>::max();
    }

    auto mesh_vertices_collision_weight_v_vs_tri = template_mesh_->get_vertex_property<double>("v:collision_weight_v_vs_tri");
    if (!mesh_vertices_collision_weight_v_vs_tri)
    {
        std::cerr << "[ERROR] update_collision_correspondences_v_vs_tri() - !mesh_vertices_collision_weight_v_vs_tri" << std::endl;
        exit(1);
        return std::numeric_limits<int>::max();
    }

    auto template_points_to_omit = template_mesh_->get_vertex_property<bool>("v:omit");

    unsigned int n_total_collisions = 0;

    std::vector<Correspondence> my_collisions_v_vs_tri;

#if _OPENMP >= 201310
#pragma omp declare reduction (merge : std::vector<Correspondence> : omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))
#pragma omp parallel for reduction(merge : my_collisions_v_vs_tri)
#endif
    for ( unsigned mesh_idx = 0; mesh_idx < template_mesh_->n_vertices(); ++mesh_idx )
    {
        Surface_mesh::Vertex v(mesh_idx);
        if (template_points_to_omit && template_points_to_omit[v])
        {
            continue;
        }

        // compute intersection from segment [p_before,p_after] with bounding mesh
        const Point p_before_def = mesh_points_before_deformation[v];
        const Point p_after_def  = template_mesh_->position(v);

        const double distance = graphene::distance(p_before_def, p_after_def);
        if (distance < 1e-6)
        {
            continue;
        }

        const Point& ray_center    = p_before_def;
        const Vec3d  ray_direction = p_after_def - p_before_def;

        Vec3d  p_intersection;
        double t_result = DBL_MAX;
        for (auto f : bounding_mesh_.faces())
        {
            auto fvit = bounding_mesh_.vertices(f);
            const auto v0 = *fvit;
            const auto v1 = *(++fvit);
            const auto v2 = *(++fvit);
            const Point tri_p0 = bounding_mesh_.position(v0);
            const Point tri_p1 = bounding_mesh_.position(v1);
            const Point tri_p2 = bounding_mesh_.position(v2);

            // bounding box of triangle
            Bounding_box bbox_tri;
            bbox_tri += tri_p0;
            bbox_tri += tri_p1;
            bbox_tri += tri_p2;
            bbox_tri.scale(1.1); // be conservative

            // trivial reject: does ray intersect bbox?
            const bool intersect_bbox = intersect_bounding_box(ray_center,
                                                               ray_direction,
                                                               bbox_tri);
            if (!intersect_bbox)
            {
                continue; // trivial reject
            }

            // does ray intersect triangle?
            Point  p_tmp;
            double t_tmp;
            if ( intersect_ray_triangle( tri_p0, tri_p1, tri_p2, ray_center, ray_direction, false, p_tmp, t_tmp) )
            {
                // is intersection closer than previous intersections?
                if (t_tmp < t_result)
                {
                    // store data of this intersection
                    t_result       = t_tmp;
                }
            }
        }

        // add valid intersections to vector of collision-correspondences
        if ( t_result <= 1.0 )
        {
            p_intersection = ray_center + t_result * Vec3f(ray_direction);

            // move outside a bit
            const Vec3d  ray_dir_normalized = (p_after_def - p_before_def).normalize();
            const double eps = 0.001;
            p_intersection -= eps*ray_dir_normalized;

            Correspondence c;
            c.on_template          = Point(0);
            c.on_template_n        = Normal(0);
            c.on_template_vh       = v;
            c.on_template_bc       = Vec3f(0);
            c.on_ps                = p_intersection;
            c.on_ps_n              = Vec3f(0);
            c.weight               = 1.0;
            c.distance_point2point = 0.0;
            c.constr_dir           = corresp_dir_mesh2ps;

            my_collisions_v_vs_tri.push_back(c);
        }
    }

    // add or update collisions_v_vs_tri
    for (auto c : my_collisions_v_vs_tri)
    {
        energy_.energy_term_data().collisions_v_vs_tri[c.on_template_vh.idx()] = c.on_ps;

        if (mesh_vertices_collision_weight_v_vs_tri[c.on_template_vh] < 1e-10) // initial case
        {
            mesh_vertices_collision_weight_v_vs_tri[c.on_template_vh] = 1000.0;
        }
        else // increase weight locally
        {
            mesh_vertices_collision_weight_v_vs_tri[c.on_template_vh] += 1000.0;
        }

        n_total_collisions++;
    }

    return n_total_collisions;
}


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


unsigned int
Template_fit::
update_collision_correspondences_tri_vs_p()
{
    if (bounding_mesh_.n_vertices() == 0)
    {
        std::cerr << "[ERROR] update_collision_correspondences_tri_vs_p() - bounding_mesh_.n_vertices() == 0" << std::endl;
        exit(1);
        return std::numeric_limits<int>::max();
    }

    auto mesh_points_before_deformation = template_mesh_->get_vertex_property<Point>("v:point_before_def");
    if (!mesh_points_before_deformation)
    {
        std::cerr << "[ERROR] update_collision_correspondences_tri_vs_p() - !mesh_points_before_deformation" << std::endl;
        exit(1);
        return std::numeric_limits<int>::max();
    }

    auto mesh_vertices_collision_weight_tri_vs_p = template_mesh_->get_vertex_property<double>("v:collision_weight_tri_vs_p");
    if (!mesh_vertices_collision_weight_tri_vs_p)
    {
        std::cerr << "[ERROR] update_collision_correspondences_tri_vs_p() - !mesh_vertices_collision_weight_tri_vs_p" << std::endl;
        exit(1);
        return std::numeric_limits<int>::max();
    }

    auto template_points_to_omit = template_mesh_->get_vertex_property<bool>("v:omit");

    unsigned int n_total_collisions = 0;

    struct Face_and_points
    {
        Surface_mesh::Face  f;
        std::vector<Point> ps;
    };

    std::vector<Face_and_points> my_collisions_tri_vs_p;

    for ( auto f : template_mesh_->faces() )
    {
        std::vector<Vec3d>                 template_ps_new_trig;
        std::vector<Vec3d>                 template_ps_old_trig;
        std::vector< Surface_mesh::Vertex > vs;
        Bounding_box bbox;
        for ( auto vf : template_mesh_->vertices(f) )
        {
            vs.push_back(vf);
            template_ps_new_trig.push_back( template_mesh_->position(vf) );
            template_ps_old_trig.push_back( mesh_points_before_deformation[vf] );

            bbox += template_mesh_->position(vf);
            bbox += mesh_points_before_deformation[vf];
        }
        bbox.scale(1.1); // be conservative

        bool omit_face = false;
        if (template_points_to_omit)
        {
            if (template_points_to_omit[vs[0]] ||
                template_points_to_omit[vs[1]] ||
                template_points_to_omit[vs[2]])
            {
                omit_face = true;
            }
        }
        if (omit_face)
        {
            continue;
        }

        // const double distance = graphene::distance(p_before_def, p_after_def);
        // if (distance < 1e-6)
        // {
        //     continue;
        // }

        for ( auto v_tar : bounding_mesh_.vertices() )
        {
            const Point& p_float = bounding_mesh_.position(v_tar);
            const Vec3d p(p_float);

            if (!bbox.is_inside(p_float))
            {
                continue;
            }

            std::vector<Point> solved_positions;
            bool did_collide =
                did_triangle_collide_with_point_and_resolve(template_ps_old_trig,
                                                            template_ps_new_trig,
                                                            p, solved_positions);

            if (did_collide)
            {
                if (solved_positions.size() != 3)
                {
                    std::cerr << "solved_positions.size() != 3" << std::endl;
                    exit(1);
                }
                if (vs.size() != 3)
                {
                    std::cerr << "vs.size() != 3" << std::endl;
                    exit(1);
                }

                Face_and_points current_face_and_points;
                current_face_and_points.f = f;
                current_face_and_points.ps.push_back(solved_positions[0]);
                current_face_and_points.ps.push_back(solved_positions[1]);
                current_face_and_points.ps.push_back(solved_positions[2]);

                my_collisions_tri_vs_p.push_back(current_face_and_points);
            }
        }
    }

    // aggregate points for individual vertices
    // fill map
    std::unordered_map< int, std::vector<Point> > map_my_collisions_tri_vs_p;
    for (auto c : my_collisions_tri_vs_p)
    {
        Surface_mesh::Face f(c.f);
        unsigned int i = 0;
        for (auto vf : template_mesh_->vertices(f))
        {
            map_my_collisions_tri_vs_p[vf.idx()].push_back(c.ps[i]);
            ++i;
        }
    }

    // add or update collisions
    for (auto c : map_my_collisions_tri_vs_p)
    {
        const std::vector<Point>& ps = c.second;
        if (ps.size() == 0)
        {
            std::cerr << "ps.size() == 0" << std::endl;
            exit(1);
        }
        Point p(0);
        for (auto current_p : ps)
        {
            p += current_p;
        }
        p /= ps.size();

        energy_.energy_term_data().collisions_tri_vs_p[c.first] = p;

        if (mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(c.first)] < 1e-10) // initial case
        {
            mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(c.first)] = 1000.0;
        }
        else // increase weight locally
        {
            mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(c.first)] += 1000.0;
            mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(c.first)] = std::min(
                mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(c.first)],
                10000.0
                );
        }

        n_total_collisions++;
    }

    return n_total_collisions;
}


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


void
Template_fit::
collision_resolve_v_vs_tri()
{
    if (bounding_mesh_.n_vertices() == 0)
    {
        std::cerr << "[ERROR] collision_resolve_v_vs_tri() - bounding_mesh_.n_vertices() == 0" << std::endl;
        exit(1);
        return;
    }

    auto mesh_points_before_deformation = template_mesh_->get_vertex_property<Point>("v:point_before_def");
    if (!mesh_points_before_deformation)
    {
        std::cerr << "[ERROR] collision_resolve_v_vs_tri() - !mesh_points_before_deformation" << std::endl;
        exit(1);
        return;
    }

    auto template_points_to_omit = template_mesh_->get_vertex_property<bool>("v:omit");

    unsigned int n_intersections = 0;

#pragma omp parallel for
    for ( unsigned mesh_idx = 0; mesh_idx < template_mesh_->n_vertices(); ++mesh_idx )
    {
        Surface_mesh::Vertex v(mesh_idx);
        if (template_points_to_omit && template_points_to_omit[v])
        {
            continue;
        }

        // compute intersection from segment [p_before,p_after] with bounding mesh
        const Point p_before_def = mesh_points_before_deformation[v];
        const Point p_after_def  = template_mesh_->position(v);

        const double distance = graphene::distance(p_before_def, p_after_def);
        if (distance < 1e-6)
        {
            // continue;
        }

        // bounding box of ray
        // Bounding_box bbox_ray;
        // bbox_ray += p_before_def;
        // bbox_ray += p_after_def;

        const Point& ray_center    = p_before_def;
        const Vec3d  ray_direction = p_after_def - p_before_def;

        Vec3d  p_intersection;
        double t_result = DBL_MAX;
        for (auto f : bounding_mesh_.faces())
        {
            auto fvit = bounding_mesh_.vertices(f);
            const auto v0 = *fvit;
            const auto v1 = *(++fvit);
            const auto v2 = *(++fvit);
            const Point tri_p0 = bounding_mesh_.position(v0);
            const Point tri_p1 = bounding_mesh_.position(v1);
            const Point tri_p2 = bounding_mesh_.position(v2);

            // bounding box of triangle
            Bounding_box bbox_tri;
            bbox_tri += tri_p0;
            bbox_tri += tri_p1;
            bbox_tri += tri_p2;
            bbox_tri.scale(1.1); // be conservative

            // trivial reject: does ray intersect bbox?
            const bool intersect_bbox = intersect_bounding_box(ray_center,
                                                               ray_direction,
                                                               bbox_tri);
            if (!intersect_bbox)
            {
                continue; // trivial reject
            }

            // does ray intersect triangle?
            Point  p_tmp;
            double t_tmp;
            if ( intersect_ray_triangle( tri_p0, tri_p1, tri_p2, ray_center, ray_direction, false, p_tmp, t_tmp) )
            {
                // is intersection closer than previous intersections?
                if (t_tmp < t_result)
                {
                    // store data of this intersection
                    t_result       = t_tmp;
//                    p_intersection = Vec3d(p_tmp);
                }
            }
        }

        if (t_result < 0.0)
        {
            std::cerr << "[ERROR] brteklasgd - t_result < 0.0" << std::endl;
            exit(1);
        }

        // add valid intersections to vector of collision-correspondences
        if ( t_result <= 1.0 /*t_result != DBL_MAX*/ )
        {
            p_intersection = ray_center + t_result * Vec3f(ray_direction);

            // move outside a bit
            const Vec3d  ray_dir_normalized = (p_after_def - p_before_def).normalize();
            const double eps = 0.001;
            p_intersection -= eps*ray_dir_normalized;

            // error computation
            Vec3d p_before = template_mesh_->position(v);

            // hard resolve
            template_mesh_->position(v) = p_intersection;

#pragma omp critical
            {
                n_intersections++;
            }
        }
    }
}


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


void
Template_fit::
collision_resolve_tri_vs_p()
{
    if (bounding_mesh_.n_vertices() == 0)
    {
        std::cerr << "[ERROR] collision_resolve_tri_vs_p() - bounding_mesh_.n_vertices() == 0" << std::endl;
        exit(1);
        return;
    }

    auto mesh_points_before_deformation = template_mesh_->get_vertex_property<Point>("v:point_before_def");
    if (!mesh_points_before_deformation)
    {
        std::cerr << "[ERROR] collision_resolve_tri_vs_p() - !mesh_points_before_deformation" << std::endl;
        exit(1);
        return;
    }

    auto template_points_to_omit = template_mesh_->get_vertex_property<bool>("v:omit");

    struct Face_and_points
    {
        Surface_mesh::Face  f;
        std::vector<Point> ps;
    };

    unsigned int n_intersections       = 0;
    unsigned int n_intersections_local = 0;
    unsigned int min_n_intersections_local = std::numeric_limits<unsigned int>::max();
    unsigned int cnt_no_decrease           = 0;

    do
    {
        std::vector<Face_and_points> my_collisions;

        n_intersections_local = 0;

        for ( unsigned int f_idx = 0; f_idx < template_mesh_->n_faces(); ++f_idx )
        {
            const Surface_mesh::Face f(f_idx);

            std::vector<Vec3d>                  template_ps_new_trig;
            std::vector<Vec3d>                  template_ps_old_trig;
            std::vector< Surface_mesh::Vertex > vs;
            Bounding_box bbox;
            for ( auto vf : template_mesh_->vertices(f) )
            {
                vs.push_back(vf);
                template_ps_new_trig.push_back( template_mesh_->position(vf) );
                template_ps_old_trig.push_back( mesh_points_before_deformation[vf] );

                bbox += template_mesh_->position(vf);
                bbox += mesh_points_before_deformation[vf];
            }
            bbox.scale(1.1); // be conservative

            bool omit_face = false;
            if (template_points_to_omit)
            {
                for (auto v : vs)
                {
                    if (template_points_to_omit[v])
                    {
                        omit_face = true;
                        break;
                    }
                }
            }
            if (omit_face)
            {
                continue;
            }

            // const double distance = graphene::distance(p_before_def, p_after_def);
            // if (distance < 1e-6)
            // {
            //     continue;
            // }

            for ( auto v_tar : bounding_mesh_.vertices() )
            {
                const Point& p_float = bounding_mesh_.position(v_tar);
                const Vec3d p(p_float);

                if (!bbox.is_inside(p_float))
                {
                    continue;
                }

                std::vector<Point> solved_positions;
                const bool did_collide =
                    did_triangle_collide_with_point_and_resolve(template_ps_old_trig,
                                                                template_ps_new_trig,
                                                                p, solved_positions);

                if (did_collide)
                {
                    if (solved_positions.size() != 3)
                    {
                        std::cerr << "solved_positions.size() != 3" << std::endl;
                        std::cerr << "solved_positions.size(): " << solved_positions.size() << std::endl;
                        for (unsigned int k = 0; k < solved_positions.size(); ++k)
                        {
                            std::cerr << "solved_positions[k]: " << solved_positions[k] << std::endl;
                        }
                        exit(1);
                    }
                    if (vs.size() != 3)
                    {
                        std::cerr << "vs.size() != 3" << std::endl;
                        exit(1);
                    }

                    Face_and_points current_face_and_points;
                    current_face_and_points.f = f;
                    current_face_and_points.ps.push_back(solved_positions[0]);
                    current_face_and_points.ps.push_back(solved_positions[1]);
                    current_face_and_points.ps.push_back(solved_positions[2]);

                    my_collisions.push_back(current_face_and_points);

                    n_intersections++;
                    n_intersections_local++;
                }
            }
        }

        const unsigned int before = min_n_intersections_local;
        min_n_intersections_local = std::min(min_n_intersections_local, n_intersections_local);
        if (min_n_intersections_local >= before) // no decrease
        {
            cnt_no_decrease++;
        }

        // aggregate points for individual vertices
        // fill map
        std::unordered_map< int, std::vector<Point> > map_my_collisions;
        for (auto c : my_collisions)
        {
            Surface_mesh::Face f(c.f);
            unsigned int i = 0;
            for (auto vf : template_mesh_->vertices(f))
            {
                map_my_collisions[vf.idx()].push_back(c.ps[i]);
                ++i;
            }

        }

        // resolve collisions
        for (auto c : map_my_collisions)
        {
            const std::vector<Point>& ps = c.second;
            if (ps.size() == 0)
            {
                std::cerr << "ps.size() == 0" << std::endl;
                exit(1);
            }
            Point p(0);
            for (auto current_p : ps)
            {
                p += current_p;
            }
            p /= ps.size();

            // hard resolve
            template_mesh_->position(Surface_mesh::Vertex(c.first)) = p;
        }
    }
    while (n_intersections_local > 0 && cnt_no_decrease <= 10);
}


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


void
Template_fit::
update_area_property()
{
    auto area  = template_mesh_->vertex_property<double>("v:area");
    // compute the voronoi area for the vertex
    double& surface_area = energy_.energy_term_data().surface_area;
    surface_area = 0.0;
    for (auto v : template_mesh_->vertices())
    {
        area[v] = voronoi_area(*template_mesh_, v);
        surface_area += area[v];
    }
}


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


void
Template_fit::
update_cotan_property()
{
    auto cotan = template_mesh_->edge_property<double>("e:cotan");
    // compute the cot weights for the edges
    for (auto e : template_mesh_->edges())
    {
//        cotan[e] = std::max(0.0, cotan_weight(*template_mesh_, e)); // DANN IST ES NAEHER DRAN AM EDGE-LAPLACE ABER DAS IST GANZ GANZ WICHTIG DA SONST DIE ARAP ENERGIE NEGATIV WERDEN KANN !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! ALSO SOOOOOOO MACHEN !!!!!!!!!
        cotan[e] = cotan_weight(*template_mesh_, e); // KLAPPT AUCH / ABER NICHT BEI ORIG STRUCT ENERGIE, DAHER CLAMPE ICH DORT

//        cotan[e] = graphene::surface_mesh::cotan(xp-xq, xr-xq);

    }
}


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


void
Template_fit::
update_laplaciancoordinate_property()
{
    auto template_points = template_mesh_->vertex_property<Point>("v:point");
    auto cotan           = template_mesh_->get_edge_property<double>("e:cotan");
    auto area            = template_mesh_->get_vertex_property<double>("v:area");
    if (!cotan || !area)
    {
        std::cerr << "[ERROR] in Template_fit::update_laplaciancoordinate_property()" << std::endl;
        return;
    }

    // compute laplacian coordinates of template vertices
    auto template_laplacian_coordinates = template_mesh_->vertex_property<Point>("v:laplacian_coordinates");
    for (auto v : template_mesh_->vertices())
    {
        Point lapl_coord(0, 0, 0);

        for (auto hc : template_mesh_->halfedges(v))
        {
            const double w    = cotan[template_mesh_->edge(hc)] / (2*area[v]);
            lapl_coord += w * (template_points[template_mesh_->to_vertex(hc)] - template_points[v]);
        }

        template_laplacian_coordinates[v] = lapl_coord;
    }
}


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


void
Template_fit::
update_aniso_laplace_properties()
{
    auto cotan            = template_mesh_->halfedge_property<double>("fitting:cotan");
    auto area             = template_mesh_->edge_property<double>("fitting:area");
    auto inner_edges_prop = template_mesh_->get_mesh_property< std::vector< Surface_mesh::Edge > >("inner_edges");
    if (!inner_edges_prop)
    {
        inner_edges_prop = template_mesh_->add_mesh_property< std::vector< Surface_mesh::Edge > >("inner_edges");
    }
    if (!cotan || !area || !inner_edges_prop)
    {
        std::cerr << "[ERROR] in Template_fit::update_aniso_laplace_properties()" << std::endl;
        return;
    }


    // compute cotan of each triangle corner (identified by halfedge pointing to it)
    for ( auto hit : template_mesh_->halfedges() )
    {
        if ( template_mesh_->is_boundary( hit ) )
        {
            cotan[hit] = 0.0;
        }
        else
        {
            auto vp = template_mesh_->from_vertex( hit );
            auto vq = template_mesh_->to_vertex( hit );
            auto vr = template_mesh_->to_vertex( template_mesh_->next_halfedge( hit ) );

            const Point& xp = template_mesh_->position(vp);
            const Point& xq = template_mesh_->position(vq);
            const Point& xr = template_mesh_->position(vr);

            // const double cot = graphene::surface_mesh::cotan(xp-xq, xr-xq);
            // cotan[hit] = std::max( 0.0, clamp_cot(cot) );
            cotan[hit] = graphene::surface_mesh::cotan(xp-xq, xr-xq);
        }
    }


    // compute area associated to each edge (1/3 of area of incident triangles)
    double &surface_area_aniso = energy_.energy_term_data().surface_area_aniso;
    surface_area_aniso = 0.0;
    for ( auto e : template_mesh_->edges() )
    {
        if ( template_mesh_->is_boundary( e ))
        {
            area[e] = 0.0;
        }
        else
        {
            auto h = template_mesh_->halfedge(e, 0);

            auto vp = template_mesh_->from_vertex(h);
            auto vq = template_mesh_->to_vertex(h);
            auto vr = template_mesh_->to_vertex( template_mesh_->next_halfedge(h) );
            auto vs = template_mesh_->to_vertex( template_mesh_->next_halfedge( template_mesh_->opposite_halfedge(h) ) );

            const Point& xp = template_mesh_->position(vp);
            const Point& xq = template_mesh_->position(vq);
            const Point& xr = template_mesh_->position(vr);
            const Point& xs = template_mesh_->position(vs);

            area[e] = (triangle_area(xp,xq,xr) + triangle_area(xp,xq,xs)) / 3.0;
            surface_area_aniso += area[e];
        }
    }


    // compute aniso laplacians (edges)
    auto template_aniso_laplace = template_mesh_->edge_property<Point>("e:aniso_laplace");
    auto template_points        = template_mesh_->vertex_property<Point>("v:point");

    // collect and count inner edges: each one provides a smoothing constraint
    std::vector< Surface_mesh::Edge > inner_edges;
    inner_edges.reserve( template_mesh_->n_edges() );
    for ( auto e : template_mesh_->edges() )
    {
        if ( ! template_mesh_->is_boundary(e) )
        {
            inner_edges.push_back( e );
        }
    }
    inner_edges_prop[0] = inner_edges;

    auto first_template_aniso_laplace = template_mesh_->edge_property<Point>("e:aniso_laplace_initial");

    const unsigned int ne = inner_edges_prop[0].size();
    for ( unsigned int i = 0; i < ne; ++i )
    {
        auto h = template_mesh_->halfedge( inner_edges_prop[0][i], 0 );

        auto vp = template_mesh_->from_vertex( h );
        auto vq = template_mesh_->to_vertex( h );
        auto vr = template_mesh_->to_vertex( template_mesh_->next_halfedge(h) );
        auto vs = template_mesh_->to_vertex( template_mesh_->next_halfedge( template_mesh_->opposite_halfedge(h) ) );

        // cotan values for edge-incident angles
        const double wik = cotan[ h ];
        const double wij = cotan[ template_mesh_->prev_halfedge(h) ];
        const double wim = cotan[ template_mesh_->opposite_halfedge(h) ];
        const double wil = cotan[ template_mesh_->prev_halfedge( template_mesh_->opposite_halfedge(h) ) ];
        const double l_area = area[ inner_edges_prop[0][i] ];

        // weights for edge-based mean curvature vector
        double ws =  (wil + wim) / l_area;
        double wp = -(wik + wil) / l_area;
        double wr =  (wik + wij) / l_area;
        double wq = -(wij + wim) / l_area;


        // compute laplace(current edge)
        Point lapl_xe(0, 0, 0);
        lapl_xe += wq * template_points[vq];
        lapl_xe += ws * template_points[vs];
        lapl_xe += wr * template_points[vr];
        lapl_xe += wp * template_points[vp];

        if (!helper_use_old_laplacian_)
        {
            template_aniso_laplace[ inner_edges_prop[0][i] ] = lapl_xe;
        }
        else
        {
            float amount_initial_laplace = 0.15f;
            template_aniso_laplace[ inner_edges_prop[0][i] ] = (1.0f - amount_initial_laplace) * lapl_xe + amount_initial_laplace * first_template_aniso_laplace[ inner_edges_prop[0][i] ];
        }
    }

    if (!helper_use_old_laplacian_)
    {
        for (auto e : template_mesh_->edges())
        {
            first_template_aniso_laplace[e] = template_aniso_laplace[e];
        }

        // note: comment out if you don't want to use original laplace to a certain extent
        //helper_use_old_laplacian_ = true;
    }
}


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