//== 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 <exact_ccd/rootparitycollisiontest.h>

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


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


namespace graphene {
namespace surface_mesh {


using graphene::geometry::Bounding_box;


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


Template_fit::
Template_fit() :
    point_set_(NULL),
    template_mesh_(NULL),
    fn_folder_fieldliner_("")
{
}


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


void
Template_fit::
clear()
{
    // invalidate point_set_
    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)
{
    //    std::cout << "[INFO] Template_fit::set_point_set. Address: " << point_set << std::endl;


    point_set_ = point_set;
    energy_.energy_term_data().point_set = point_set;
/*
    if (point_set_ != NULL)
    {
        std::cout << "[INFO] Template_fit::set_point_set. #point: " << point_set_->points_.size() << std::endl;
    }
*/
}


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


void
Template_fit::
set_template_mesh(Surface_mesh* template_mesh)
{
//    std::cout << "[INFO] Template_fit::set_template_mesh. Address: " << template_mesh << std::endl;



    template_mesh_ = template_mesh;

    energy_.energy_term_data().template_mesh = template_mesh;
/*
    if (template_mesh_ != NULL)
    {
        std::cout << "[INFO] Template_fit::set_template_mesh. #vertices: " << template_mesh_->n_vertices() << std::endl;
    }
*/
//    update_restpose();
    //            energy_.energy_term_data().eyes_manager.set_template_points_eyecontour( *template_mesh );
}


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


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 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;
            c.on_ps_n              = Vec3f(0);
            c.weight               = 1.0;
            c.distance_point2point = distance( template_mesh_->position(v), p );
            c.constr_dir           = corresp_dir_mesh2ps;

            if (template_points_to_omit)
            {
                if (!template_points_to_omit[v])
                {
                    energy_.energy_term_data().fieldlines.push_back(c);
                }
            }
            else
            {
                energy_.energy_term_data().fieldlines.push_back(c);
            }
        }
    }
    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);
    }
    std::cerr << "[DEBUGGG] dist_min: " << dist_min << std::endl;
    std::cerr << "[DEBUGGG] dist_max: " << dist_max << std::endl;
    if (dist_min >= dist_max)
    {
        std::cerr << "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]

        if (s < 0)
        {
            std::cerr << "i: " << i << std::endl;
            std::cerr << "s: " << s << std::endl;
            std::cerr << "corr.weight before: " << corr.weight << std::endl;
        //     // std::cerr << "corr.weight after: " << corr.weight << std::endl;
        //     std::cerr << std::endl;
            exit(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)
{
    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_vertexweighting: [ERROR] No template mesh available." << std::endl;
        return false;
    }

    std::ifstream ifs( filename.c_str() );
    if( ifs.is_open() != true )
    {
        std::cerr << "Template_fit::load_vertexweighting: [ERROR] Cannot load weighting from file:\"" << filename << "\"." << std::endl;
        return false;
    }

    //perform sanity check before load
    //count lines and check if #lines == #vertices of mesh
    size_t n_lines = 0;
    std::string line;
    while (ifs)
    {
        std::getline(ifs, line);
        if (line.empty())
            break;

        ++n_lines;
    }

    if (n_lines != template_mesh_->n_vertices())
    {
        std::cerr << "Template_fit::load_vertexweighting: [ERROR] Wrong number of vertexweightings " << n_lines << " vs " << template_mesh_->n_vertices() << "." << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<double> property_of_interest = template_mesh_->vertex_property<double>(property_name.c_str(), 1.0);

    //clear and rewind to beginning of file
    ifs.clear();
    ifs.seekg(0);

    double value;

    if (multiply)
    {
        for (auto v : template_mesh_->vertices())
        {
            ifs >> value;
            if (ifs.eof()) break;
            property_of_interest[v] *= value;
        }
    }
    else
    {
        for (auto v : template_mesh_->vertices())
        {
            ifs >> value;
            if (ifs.eof()) break;
            property_of_interest[v] = value;
        }
    }
    return true;
}


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

bool
Template_fit::
load_vertexweighting(const std::string& filename, const std::string& filename_mapping, const std::string& property_name, bool multiply)
{
    if (template_mesh_ == NULL)
    {
        std::cerr << "Template_fit::load_vertexweighting: [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;
    }

    std::ifstream ifs( filename.c_str() );
    if( ifs.is_open() != true )
    {
        std::cerr << "Template_fit::load_vertexweighting: [ERROR] Cannot load weighting from file:\"" << filename << "\"." << std::endl;
        return false;
    }

    // load values
    std::vector<double> values_vec;
    double value;
    while (true)
    {
        ifs >> value;
        if (!ifs) break;

        values_vec.push_back(value);
    }

    //perform sanity check
    //#valid_mappingss == #vertices of mesh
    size_t n_lines = 0;
    for (unsigned int i = 0; i < mapping_vec.size(); ++i)
    {
        if (mapping_vec[i] == -1)
        {
            continue;
        }

        ++n_lines;
    }
    if (n_lines != template_mesh_->n_vertices())
    {
        std::cerr << "Template_fit::load_vertexweighting: [ERROR] Wrong number of valid mappings " << n_lines << " vs " << template_mesh_->n_vertices() << "." << std::endl;
        return false;
    }
    if (mapping_vec.size() != values_vec.size())
    {
        std::cerr << "Template_fit::load_vertexweighting: [ERROR] Can't load vertexweighting: mapping_vec.size() != values_vec.size()." << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<double> property_of_interest = template_mesh_->vertex_property<double>(property_name.c_str(), 1.0);

    if (multiply)
    {
        for (unsigned int i = 0; i < mapping_vec.size(); ++i)
        {
            const int map = mapping_vec[ i ];

            if (map != -1)
            {
                property_of_interest[Surface_mesh::Vertex(map)] *= values_vec[i];
            }
        }
    }
    else
    {
        for (unsigned int i = 0; i < mapping_vec.size(); ++i)
        {
            const int map = mapping_vec[ i ];

            if (map != -1)
            {
                property_of_interest[Surface_mesh::Vertex(map)] = values_vec[i];
            }
        }
    }

    return true;
}

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

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

    Surface_mesh::Vertex_property<double> property_of_interest = template_mesh_->get_vertex_property<double>(property_name.c_str());

    if (property_of_interest)
    {
        template_mesh_->remove_vertex_property<double>(property_of_interest);
    }

    return true;

}

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


bool
Template_fit::
do_similarity_transformation_lm(const Transformation_direction trans_dir)
{
    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_))
    {
        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);
        // landmarks_manager.get_landmarks_template_stdvec(template_mesh_, lm_templ);

        const Mat4f M = geometry::registration(lm_templ,
                                               lm_ps,
                                               graphene::geometry::CONFORMAL_REGISTRATION);
        std::cout << M << std::endl;

        // 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);
        // 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& constr_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> correspondences;
        if (!compute_correspondences(template_mesh_, point_set_, constr_settings, correspondences))
        {
            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 < correspondences.size(); ++i)
        {
            points_ps.push_back(correspondences[i].on_ps);
            points_template.push_back(correspondences[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(const Correspondences_settings &constr_settings,
      const unsigned int max_iter,
      const double conv_val,
      const double until_lambda,
      const bool make_plastic,
      const bool no_decrease)
{
    if (template_mesh_ == NULL || point_set_ == NULL)
    {
        std::cerr << "Template_fit::start(): [ERROR] Either no template or no point set present. Aborting..." << std::endl;
        return;
    }

    update_restpose();

    bool      must_compute_cpc_corresp = 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;

    //TODO Do we need to check valid settings here?!
    //check_if_settings_are_valid( constr_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();
    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");

    const int n_steps_fl_incr = 3; // 10: from 1.0 to 1e-9 OR X: from 1e-7 to 1e-9
    const float fl_incr_start = 0.4f;
    const float fl_incr_last  = 1.0f;
    int         cnt_fl_incr   = 0;

    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
            system( command_fieldliner.str().c_str() );

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

            // update local weights for FLCs
            // update_local_weights_flcs();
            // std::cerr << "[DEBUG TEMP] UPDATE LOCAL WEIGHTS FOR FLCs FINISHED!" << std::endl;
        }

        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 (!compute_correspondences(template_mesh_, point_set_, constr_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 ))
            {
                std::cerr << "Template_fit::start(): [ERROR] Minimize failed -> Aborting..." << std::endl;
                return;
            }

            // ------------------------------------------------------------------------------------------- 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 (/*!second_loop_done &&*/ must_handle_collisions) // TODO ATTENTION: take care of max_iter (several times)
            {
                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_constr_weights = energy_.energy_term_data().sum_constr_weights;
    sum_constr_weights = 0.0;
    for (auto c : correspondences)
    {
        sum_constr_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");

    // 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 bla = 0; bla < template_mesh_->n_vertices(); ++bla ) // auto v : template_mesh_->vertices()
    {
        Surface_mesh::Vertex v(bla);
        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; // TODO WAR 200.0
        }
        else // increase weight locally
        {
            mesh_vertices_collision_weight_v_vs_tri[c.on_template_vh] += 1000.0; // TODO WAR 200.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_easy_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; // TODO WAR 200.0
        }
        else // increase weight locally
        {
            mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(c.first)] += 1000.0; // TODO WAR 200.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 // TODO WAR 600.0; und auch mal 5000.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 bla = 0; bla < template_mesh_->n_vertices(); ++bla ) // auto v : template_mesh_->vertices()
    {
        Surface_mesh::Vertex v(bla);
        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_easy_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);
}


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


bool
Template_fit::
did_triangle_collide_with_point_easy_resolve(const std::vector<Vec3d>& old_trig,
                                             const std::vector<Vec3d>& new_trig,
                                             const Vec3d& p,
                                             std::vector<Point>& _solved_positions)
{
    if ( old_trig.size() != 3 || new_trig.size() != 3 )
    {
        std::cerr << "Problem: wrong size of trig vector!!" << std::endl;
        return false;
    }

    // convert pmp vectors in collision detection vectors
    CCD::Vec3d
            x0(p.data()),
            x1(old_trig[0].data()),
            x2(old_trig[1].data()),
            x3(old_trig[2].data()),
            x0n(p.data()),
            x1n(new_trig[0].data()),
            x2n(new_trig[1].data()),
            x3n(new_trig[2].data());

    // perform collision detection
    CCD::rootparity::RootParityCollisionTest ccd(x0,x1,x2,x3,x0n,x1n,x2n,x3n,false);
    bool collision = ccd.run_test();

    // perform collision resolve
    std::vector<Vec3d> solved_positions;
    if (collision)
    {
        Vec3d         n = cross(new_trig[2] - new_trig[0], new_trig[1] - new_trig[0]);
        Vec3d solve_vec;

        if(dot(n,n) > std::numeric_limits<double>::min())
        {
            solve_vec = (dot(n,p - new_trig[0])*n/dot(n,n));
        }
        else
        {
            // take vector from old to new barycenter as normal
            n = (old_trig[0] + old_trig[1] + old_trig[2]) - (new_trig[0] + new_trig[1] + new_trig[2]);

            // check all possible displacements to make sure, we pick the correct one
            double maxd = 0;
            for(int i = 0; i < 3; i++) maxd = std::max(maxd,dot(n,p - new_trig[i]));

            solve_vec = maxd*n/dot(n,n);
        }

        double l = std::max(norm(solve_vec),1e-8);

        const double SAFTY_THRESHOLD(0.001); // (set the solved positions 1mm from collision position)

        solved_positions.resize(3);
        for(int i = 0; i < 3; i++)
        {
            solved_positions[i] = new_trig[i] + (l + SAFTY_THRESHOLD)*solve_vec/l;
        }

        _solved_positions.resize(3);
        _solved_positions[0] = solved_positions[0];
        _solved_positions[1] = solved_positions[1];
        _solved_positions[2] = solved_positions[2];
    }

    return collision;
}


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


bool
Template_fit::
did_triangle_collide_with_point_better_resolve(const std::vector<Vec3d>& old_trig,
                                               const std::vector<Vec3d>& new_trig,
                                               const Vec3d& p,
                                               std::vector<Point>& _solved_positions)
{
    if ( old_trig.size() != 3 || new_trig.size() != 3 )
    {
        std::cerr << "Problem: wrong size of trig vector!!" << std::endl;
        return false;
    }

    // convert pmp vectors in collision detection vectors
    CCD::Vec3d
            x0(p.data()),
            x1(old_trig[0].data()),
            x2(old_trig[1].data()),
            x3(old_trig[2].data()),
            x0n(p.data()),
            x1n(new_trig[0].data()),
            x2n(new_trig[1].data()),
            x3n(new_trig[2].data());

    // perform collision detection
    CCD::rootparity::RootParityCollisionTest ccd(x0,x1,x2,x3,x0n,x1n,x2n,x3n,false);
    bool collision = ccd.run_test();

    // perform collision resolve
    std::vector<Vec3d> solved_positions;
    if (collision)
    {
        // find longest old-to-new-edge
        int max_i = 0;
        double max_dist = 0;
        for (int i = 0; i < 3; i++)
        {
            double dist = sqrnorm(old_trig[i] - new_trig[i]);
            if (dist > max_dist)
            {
                max_dist = dist;
                max_i = i;
            }
        }

        // if max_edge is too short, stay at old triangles position
        if (max_dist < std::numeric_limits<double>::min())
        {
             solved_positions = old_trig;

             _solved_positions.resize(3);
             _solved_positions[0] = solved_positions[0];
             _solved_positions[1] = solved_positions[1];
             _solved_positions[2] = solved_positions[2];

             return true;
        }

        // else finde interpolated normal
        Vec3d  d = new_trig[max_i] - old_trig[max_i];
        double t = std::max(0.0,std::min(1.0,dot(p - old_trig[max_i], d)/dot(d,d)));

        Vec3d p0t = (1-t)*old_trig[0] + t*new_trig[0];
        Vec3d p1t = (1-t)*old_trig[1] + t*new_trig[1];
        Vec3d p2t = (1-t)*old_trig[2] + t*new_trig[2];
        Vec3d n = normalize(cross(p1t - p0t, p2t - p0t));

        // (set the solved positions 1mm from collision position)
        const double SAFTY_THRESHOLD(0.001);
        Vec3d safty_p = p + SAFTY_THRESHOLD*n;

        // set solved position to the intersection points of vertex movement lines and plane defined by normal n and point p
        solved_positions.resize(3);
        for(int i = 0; i < 3; i++)
        {
            double t2 = std::max(0.0,std::min(1.0,dot(safty_p - old_trig[i],n)/dot(new_trig[i] - old_trig[i],n)));
            solved_positions[i] = old_trig[i] + t2*(new_trig[i] - old_trig[i]);
        }

        _solved_positions.resize(3);
        _solved_positions[0] = solved_positions[0];
        _solved_positions[1] = solved_positions[1];
        _solved_positions[2] = solved_positions[2];
    }

    return collision;
}


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


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;
   
    // TODO KAKA KEEP ELASTIC BLA
    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 (!KAKAKA_BAK_LAP_THERE)
        {
            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] ];
        }
    }

    // TODO KAKA KEEP ELASTIC BLA
    if (!KAKAKA_BAK_LAP_THERE)
    {
        for (auto e : template_mesh_->edges())
        {
            first_template_aniso_laplace[e] = template_aniso_laplace[e];
        }
// TODO COMMENT OUT, I.E., DO NOT USE ORIG LAPLACE
        KAKAKA_BAK_LAP_THERE = true;
    }
}


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