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


#include "IKPose_fit.h"

#include <graphene/geometry/registration.h>
#include <graphene/geometry/Matrix3x3.h>
#include <graphene/surface_mesh/algorithms/templatefitting/Energy.h>
#include <graphene/surface_mesh/algorithms/surface_mesh_tools/crease_normals.h>
#include <graphene/surface_mesh/algorithms/surface_mesh_tools/misc.h>
#include <graphene/surface_mesh/algorithms/surface_mesh_tools/meanvalue_coords.h>
#include <graphene/surface_mesh/algorithms/templatefitting/utility/my_helper.h>
#include <graphene/surface_mesh/algorithms/templatefitting/Landmarks_manager.h>
#include <graphene/character/data_structure/IO_pose_vector.h>

#include <alglib-3.16.0/stdafx.h>
#include <alglib-3.16.0/optimization.h>

#include <fstream>
#include <sstream>


//== NAMESPACES ================================================================
namespace graphene {
namespace surface_mesh {
//=============================================================================


int IKPose_fit::DEBUG_ITER_COUNT = 0;


IKPose_fit::
IKPose_fit(character::Character* character,
           std::vector<Fitting_target*>& fitting_targets)
  :
    max_iterations_(100),
    conv_value_(0.05),
    max_normal_angle_(50.0),
    max_dist_(0.1),
    reset_to_pca_mean_(true),
    with_pca_(false),
    use_landmarks_(false),
    use_correspondences_ik_(false),
    use_correspondences_pca_(false),
    fixed_correspondences_(nullptr),
    is_initialized_(false),
    do_similarity_transform_(true),
    character_(NULL),
    fitting_targets_(fitting_targets),
    render_callback_(nullptr)
{
    reset_convergence_state();

    for (size_t i = 0; i < fitting_targets_.size(); ++i)
    {
        auto* ik_solver = new ik::Inverse_kinematics();
        ik_solver->set_clamping(true);

        ik_solvers_.push_back(ik_solver);
    }

    if (!set_character(character))
    {
        std::cerr <<"IKPose_fit::IKPose_fit: [ERROR] Can't set character!" << std::endl;
    }

    init();
}


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


IKPose_fit::
~IKPose_fit()
{
    for (size_t i = 0; i < ik_solvers_.size(); ++i)
    {
        delete ik_solvers_[i];
    }

    energy_.clear();
}

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

void
IKPose_fit::
reset_convergence_state()
{
    iter_       = 0;
    error_      = DBL_MAX;
    error_prev_ = 1.0;
    rel_error_  = DBL_MAX;
}

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

bool
IKPose_fit::
set_character(character::Character* character)
{
    character_ = character;

    if (character_ == NULL)
    {
        std::cerr <<"IKPose_fit::set_character: [ERROR] Character is NULL!" << std::endl;
        return false;
    }

    for (size_t i = 0; i < ik_solvers_.size(); ++i)
    {
        auto* ik_solver = ik_solvers_[i];
        ik_solver->clear_effectors();
        ik_solver->set_skeleton(&character_->skeleton());
    }

    return update_dof();
}

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

bool
IKPose_fit::
update_dof(const std::string& fn_template_ik_dof)
{
    for (size_t i = 0; i < ik_solvers_.size(); ++i)
    {
        std::string fn_dof = fn_template_ik_dof;
        if (fn_dof.empty())
        {
            fn_dof = fitting_targets_[i]->fn_template_ik_dof;
        }

        auto* ik_solver = ik_solvers_[i];
        if (!fn_dof.empty() && ! ik_solver->load_dof(fn_dof))
        {
            std::cerr <<"IKPose_fit::set_character: [ERROR] Can't load DoFs!" << std::endl;
            return false;
        }

        std::vector<double> prev_params = ik_solver->get_current_parameters(); // call this before calling collect_dofs()!

        if (!ik_solver->collect_dofs())
        {
            std::cerr <<"IKPose_fit::set_character: [ERROR] Can't collect DoFs!" << std::endl;
            return false;
        }

        if (prev_params.size() > 0)
        {
            if (!ik_solver->set_parameters(prev_params))
            {
                std::cerr <<"IKPose_fit::set_character: [ERROR] Can't set IK parameters!" << std::endl;
                return false;
            }
        }
    }

    return true;
}

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

void
IKPose_fit::
set_fixed_correspondences(std::vector<Correspondence>* correspondences)
{
    fixed_correspondences_ = correspondences;
}

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

void
IKPose_fit::
setup_energy_fitting_targets()
{
    auto& energy_target_data = energy_.energy_term_data().fitting_targets;
    energy_target_data.clear();

    for (auto* t : fitting_targets_)
    {
        energy_target_data.push_back(Energy_fitting_target());
        auto& target_data = energy_target_data.back();
        target_data.point_set = t->point_set;
        target_data.pose_vector = t->pose_vector;
        target_data.landmark_property_name = t->landmark_property_name;
        target_data.landmark_weight_property_name = t->landmark_weight_property_name;
    }
}

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


bool
IKPose_fit::
init()
{
    if (!is_initialized_)
    {
        if (character_ == NULL)
        {
            std::cerr <<"IKPose_fit::init: [ERROR] No character!" << std::endl;
            return false;
        }

        surface_mesh::Landmarks_manager landmarks_manager;
        if (use_landmarks_)
        {
            for (size_t i = 0; i < fitting_targets_.size(); ++i)
            {
                auto* target = fitting_targets_[i];
                if (!landmarks_manager.check_dimension(target->point_set, character_->get_selected_skin(), target->landmark_property_name))
                {
                    std::cerr <<"IKPose_fit::init: [ERROR] Landmarks dimensions do not fit!" << std::endl;
                    return false;
                }
            }
        }

        // Compute mean value coordinates in TPose
        //
        std::vector<Mat4f> pose_vector;
        surface_mesh::get_pose_vector(character_->skeleton(), pose_vector);

        std::vector<Mat4f> identity_vector(pose_vector.size(), Mat4f::identity());
        character_->apply_skinning_on_CPU(true);

        if (!surface_mesh::mvc::compute_mean_value_coords_from_current_joint_positions(character_, "m:mvc_joints"))
        {
//            std::cerr <<"IKPose_fit::init: [INFO] Can't compute MVC! There are MVCs!" << std::endl;
        }

        surface_mesh::set_pose_vector(pose_vector, character_->skeleton());
        character_->apply_skinning_on_CPU(true);

        is_initialized_ = true;
    }

    return true;
}


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


bool
IKPose_fit::
start(size_t target_index)
{
    if (!init())
    {
        std::cerr << "IKPose_fit::start: [ERROR] !init()" << std::endl;
        return false;
    }

    reset_convergence_state();

    const bool ok_minimize = minimize(target_index);

    return ok_minimize;
}


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


bool
IKPose_fit::
start_with_pca(const std::string& filename_pca_mean,
               const std::string& filename_pca,
               const std::string& filename_skinning_info_pos,
               const std::string& filename_joint_positions_chargen,
               const std::string& filename_skinning_info_wd,
               const bool shapedependent_skinningweights,
               const bool jointposcorrection_chargen,
               const double max_dist_for_pca_shape_fit,
               const int max_normal_deviation_for_pca_shape_fit,
               const int max_iter,
               const double tikhonov_weight)
{
    with_pca_ = true;

    // check for initialization
    if (!init())
    {
        std::cerr << "IKPose_fit::start_with_pca: [ERROR] !init()" << std::endl;
        return false;
    }

    // reset to *not* converged
    reset_convergence_state();

    // load skinning info
    if (shapedependent_skinningweights || jointposcorrection_chargen)
    {
        const bool ok_load_skinning_info = load_skinning_info(filename_skinning_info_pos,
                                                              filename_joint_positions_chargen,
                                                              filename_skinning_info_wd);
        if (!ok_load_skinning_info)
        {
            std::cerr << "[DEBUG] load skinning info - !ok_load_skinning_info" << std::endl;
            return false;
        }
    }

    // pointer to character for convenient access
    character::Character* template_character = character_;

    // load pca mean
    Surface_mesh mesh_pca_mean;
    if (reset_to_pca_mean_)
    {
        mesh_pca_mean.read(filename_pca_mean);
        if (0 == mesh_pca_mean.n_vertices())
        {
            std::cerr << "IKPose_fit::start_with_pca: [ERROR] PCA mean mesh is empty." << std::endl;
            return false;
        }
        mesh_pca_mean.update_face_normals();
        mesh_pca_mean.update_vertex_normals();

        // conformal registration: template model and point set to fit the pca model
        if (false)
        {
            graphene::character::Surface_mesh_skin* skin = character_->get_selected_skin();

            std::vector<Point> src_points;
            std::vector<Point> tar_points;
            for ( auto v : mesh_pca_mean.vertices() )
            {
                src_points.push_back( skin->unskinned_vertices_[v] );
                tar_points.push_back( mesh_pca_mean.position(v) );
            }

            const Mat4f M = geometry::registration(src_points,
                                                   tar_points,
                                                   graphene::geometry::CONFORMAL_REGISTRATION);

            transform_mesh(*character_->get_selected_skin(), M);
            character_->on_shape_changed_shape_in_tpose_but_skeleton_posed(true); // true: with correcting joint positions turned on

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

            // Also transform point_sets
            for (auto* fitting_target : fitting_targets_)
            {
                if (!transform_points(*(fitting_target->point_set), M))
                {
                    std::cerr << "IKPose_fit::start_with_pca: [ERROR] Can't transform points from point_set_!" << std::endl;
                    return false;
                }
            }
        }

        if (render_callback_)
            render_callback_("Posefit: Reset to pca mean");

    }

    // correspondence settings for pca-shape-fit
    Correspondences_settings correspondence_settings;
    correspondence_settings.filter_dist        = true;
    correspondence_settings.dist_until         = max_dist_for_pca_shape_fit;
    correspondence_settings.filter_normals     = true;
    correspondence_settings.normal_until       = max_normal_deviation_for_pca_shape_fit;
    correspondence_settings.std_dev_filter     = false;
    correspondence_settings.worst_nPerc_filter = false;
    correspondence_settings.boundary_filter    = true;
    correspondence_settings.symmetry_filter    = false;
    correspondence_settings.w_distance_1r      = false;
    correspondence_settings.w_distance_1ddMax  = false;
    correspondence_settings.w_dot_nn           = false;
    correspondence_settings.w_distance_huber   = false;

    correspondence_settings.constr_dir = corresp_dir_ps2mesh;//corresp_dir_mesh2ps;

    // enable/disable energy terms
    setup_energy_fitting_targets();

    auto& energy_term_list = energy_.energy_term_list();

    auto* pca_tikhonov = energy_term_list.get_term(surface_mesh::ET_TIKHONOV_PCA);
    pca_tikhonov->set_weight(tikhonov_weight);
    pca_tikhonov->set_multiply_factor(1.0);

    auto* pca_lm_term = energy_term_list.get_term(surface_mesh::ET_PCA_LANDMARKS_SKINNED_MULTIPLE_SCANS);
    pca_lm_term->set_weight(use_landmarks_ ? 0.4 : 0.0);
    pca_lm_term->set_multiply_factor(1.0);

    auto* pca_cpc_term = energy_term_list.get_term(surface_mesh::ET_PCA_CPC_PS2MESH_SKINNED_MULTIPLE_SCANS);
    pca_cpc_term->set_weight(use_correspondences_pca_ ? 1.0 : 0.0);
    pca_cpc_term->set_multiply_factor(1.0);

    // load pca model from file
    energy_.energy_term_data().pca.load_pca_model(filename_pca);
    if (energy_.energy_term_data().pca.dim_pca_model_ == 0)
    {
        std::cerr << "IKPose_fit::start_with_pca: [ERROR] PCA model has dimension 0" << std::endl;
        return false;
    }

    // TODO(swenninger): remove pca_mean_character variable, everything should be based off `skin` or `template_character`
    auto* skin = template_character->get_selected_skin();
    auto* pca_mean_character = template_character;

    if (reset_to_pca_mean_)
    {
        // copy positions (in TPose)
        skin->points() = mesh_pca_mean.points();
        pca_mean_character->on_shape_changed_shape_in_tpose_but_skeleton_posed(true); // true: with correcting joint positions turned on

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

        reset_to_pca_mean_ = false;
    }

    // compute normals for pca mean shape
    surface_mesh::crease_normals(skin, 0.0f, "h:normal");

    // set objects for template fitting (energy term data)
    energy_.energy_term_data().template_mesh = pca_mean_character->get_selected_skin();
    energy_.energy_term_data().character     = pca_mean_character;

    // set pca-character for IK pose fit
    if (! set_character(pca_mean_character) )
    {
        std::cerr << "IKPose_fit::start_with_pca: [ERROR] Can't set PCA character" << std::endl;
        return false;
    }

    //
    // @PointSetTransformation
    // NOTE(swenninger):
    // In the first iterations of the PCA + Posefit, the shape of the mesh does not necessarily
    // fit the shape of the pointcloud very well. This can lead to large errors for the
    // registration according to closest point correspondences (usually leading to an unwanted
    // downscaling of the pointcloud).
    // To compensate this, we only allow rigid transformations (=> no scaling) in the
    // closest point correspondences registration.
    // Then, after the first PCA Fit, the shape should already fit the pointcloud well enough and we go back to
    // allowing similarity transformations again.
    //
    do_similarity_transform_ = false;

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

    bool converged = false;
    int       iter = 0;
    do
    {
        //
        // Here, the character should always be in TPose
        //

        //
        // Refine pose for each target without changing the shape of the mesh
        //
        for (size_t fitting_target_index = 0;
             fitting_target_index < num_fitting_targets;
             ++fitting_target_index)
        {
            auto* target = fitting_targets_[fitting_target_index];
            auto& energy_fitting_target = energy_target_data[fitting_target_index];

            // Set character to previously fitted pose
            // pose_vector only consists of rotations
            surface_mesh::set_pose_vector(*energy_fitting_target.pose_vector, pca_mean_character->skeleton());
            pca_mean_character->apply_skinning_on_CPU(true); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals

            // Pose Fit (refine previous pose fit, based on landmarks and/or correspondences)
            if (iter > 0)
            {
                if (!start(fitting_target_index))
                {
                    std::cerr << "IKPose_fit::start_with_pca: [ERROR] !start() for fitting target " << fitting_target_index << std::endl;
                    return false;
                }

            }

            // Compute Correspondences in Scanpose
            // they are used for PCA-shape optimization
            if (use_correspondences_pca_)
            {
                if (!target->vertexweighting_property_name.empty())
                {
                    correspondence_settings.vertex_weight_property_name = target->vertexweighting_property_name;
                }
                else
                {
                    correspondence_settings.vertex_weight_property_name = "v:fitting_weight";
                }

                surface_mesh::compute_correspondences(
                        pca_mean_character->get_selected_skin(),
                        energy_fitting_target.point_set,
                        correspondence_settings,
                        energy_fitting_target.correspondences);
            }

            // Save the optimized pose and current IK parameters
            surface_mesh::get_pose_vector(pca_mean_character->skeleton(), *energy_fitting_target.pose_vector);
        }

        // See @PointSetTransformation
        if (iter == 1)
        {
            do_similarity_transform_ = true;
        }

        // Reset skeleton to TPose and skin mesh
        pca_mean_character->skeleton().reset_to_bindpose();
        pca_mean_character->apply_skinning_on_CPU(true); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals

        surface_mesh::set_pose_vector(*fitting_targets_[0]->pose_vector, pca_mean_character->skeleton());

        // optimize joint angles, shape, joint positions, joint angles, shape, joint positions, ..., and joint positions at the end
        if (converged)
        {
            break;
        }

        //
        // Find PCA Parameters that best explain the shape in T-Pose (fits shape)
        //
        energy_.minimize(0.01);
        energy_.energy_term_list().apply_weight_multipliers();

        surface_mesh::crease_normals(pca_mean_character->get_selected_skin(), 0.0f, "h:normal"); // recompute normals for pca mean shape

        // First, update skinning weights
        int idx_closest_character = -1;
        if (shapedependent_skinningweights)
        {
            // update shape-dependent skinning-weights, since shape has changed

            // update_skinning_weights_wrt_mix(pca_mean_character->get_selected_skin());
            idx_closest_character = update_skinning_weights_wrt_nearest(pca_mean_character->get_selected_skin());
            //idx_closest_character = update_skinning_weights_wrt_hardcoded_character_generator_model(pca_mean_character->get_selected_skin());

            std::cout << "IKPose_fit::start_with_pca: [STATUS] Nearest character in CharacterGenerator_DB is " <<  idx_closest_character << std::endl;
        }

        // Then, update MVCs for joint positions from Character Generator database
        if (jointposcorrection_chargen)
        {
            if (!update_joint_position_MVCs_from_chargen(pca_mean_character->get_selected_skin(), idx_closest_character))
            {
                std::cerr << "IKPose_fit::start_with_pca: [ERROR] Can't update joint positions' MVCs from CharGen!" << std::endl;
                return false;
            }
        }

        // After the PCA Shape-Fit, the joint positions are not correct anymore.
        // As we are in TPose here, it is safe and best to correct joint positions here
        // And copy unskinned properties, since we are in TPose and changed shape
        pca_mean_character->on_shape_changed_shape_in_tpose_but_skeleton_posed(true); // true: with correcting joint positions turned on

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


        if (render_callback_)
            render_callback_("Posefit: PCA step");


        ++iter;

        if (iter >= max_iter)
        {
            converged = true;
        }


    }
    while (/*rel_error > conv_val  &&*/ /*iter < max_iter*/ true );

    // Go back to TPose
    pca_mean_character->skeleton().reset_to_bindpose();
//    pca_mean_character->apply_skinning_on_CPU(true); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals
//    pca_mean_character->on_shape_changed_in_tpose(true);


    surface_mesh::crease_normals(pca_mean_character->get_selected_skin(), 0.0f, "h:normal"); // recompute normals for pca mean shape

    //
    // Here, the character is in TPose
    //

#if 0
    //
    // Copy solution from temporary pca mean character to template character (in TPose)
    //
    template_character->get_selected_skin()->points() = pca_mean_character->get_selected_skin()->points();

    // Copy normals and unskinned properties (in TPose)
    auto* template_skin = template_character->get_selected_skin();
    auto* pca_skin      = pca_mean_character->get_selected_skin();
    for (auto v : template_skin->vertices())
    {
        template_skin->unskinned_vertices_[v]  = pca_skin->unskinned_vertices_[v];
        template_skin->v_normals_[v]           = pca_skin->v_normals_[v];
        template_skin->v_unskinned_normals_[v] = pca_skin->v_unskinned_normals_[v];
    }

    template_character->skeleton() = pca_mean_character->skeleton();

    // Don't forget to copy weights/depends in case we optimized skinning weights
    if (shapedependent_skinningweights)
    {
        for (auto v : template_skin->vertices())
        {
            template_skin->v_depends1_[v]  = pca_skin->v_depends1_[v];
            template_skin->v_weights1_[v]  = pca_skin->v_weights1_[v];
            template_skin->v_depends2_[v]  = pca_skin->v_depends2_[v];
            template_skin->v_weights2_[v]  = pca_skin->v_weights2_[v];
        }
    }
    // And MVCs
    if (jointposcorrection_chargen)
    {
        auto mvc_tmp_src = pca_mean_character->get_selected_skin()->mesh_property<std::vector<std::vector<float>>>("m:mvc_joints");
        auto mvc_tmp_tar = template_character->get_selected_skin()->mesh_property<std::vector<std::vector<float>>>("m:mvc_joints");

        mvc_tmp_tar[0] = mvc_tmp_src[0];
    }
    surface_mesh::crease_normals(template_skin, 0.0f, "h:normal"); // recompute normals for pca mean shape

    // set original character for IK pose fit
    if (! set_character(template_character) )
    {
        std::cerr << "IKPose_fit::start_with_pca: [ERROR] Can't set original character" << std::endl;
        return false;
    }
#endif

    //
    // Set skeleton to Scanpose for further processing (non-rigid shape deformation)
    // Note: Don't apply skinning, i.e., the character's geometry is in TPose

    // Reset to first fitting target
    surface_mesh::set_pose_vector(*fitting_targets_[0]->pose_vector, template_character->skeleton());

    with_pca_ = false;

    return true;
}

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

bool
IKPose_fit::start_with_pca(const std::string& filename_pca_mean,
                           const std::string& filename_pca,
                           const std::string& filename_pca_mapping,
                           const double max_dist_for_pca_shape_fit,
                           const int max_normal_deviation_for_pca_shape_fit,
                           const int max_iter,
                           const double tikhonov_weight)
{
    with_pca_ = true;

    // check for initialization
    if (!init())
    {
        std::cerr << "IKPose_fit::start_with_pca: [ERROR] !init()" << std::endl;
        return false;
    }

    // reset to *not* converged
    reset_convergence_state();

    // pointer to character for convenient access
    character::Character* template_character = character_;

    // load pca mean
    Surface_mesh mesh_pca_mean;
    if (reset_to_pca_mean_)
    {
        mesh_pca_mean.read(filename_pca_mean);
        if (0 == mesh_pca_mean.n_vertices())
        {
            std::cerr << "IKPose_fit::start_with_pca: [ERROR] PCA mean mesh is empty." << std::endl;
            return false;
        }
        mesh_pca_mean.update_face_normals();
        mesh_pca_mean.update_vertex_normals();
    }

    // correspondence settings for pca-shape-fit
    Correspondences_settings correspondence_settings;
    correspondence_settings.filter_dist        = true;
    correspondence_settings.dist_until         = max_dist_for_pca_shape_fit;
    correspondence_settings.filter_normals     = true;
    correspondence_settings.normal_until       = max_normal_deviation_for_pca_shape_fit;
    correspondence_settings.std_dev_filter     = false;
    correspondence_settings.worst_nPerc_filter = false;
    correspondence_settings.boundary_filter    = true;
    correspondence_settings.symmetry_filter    = false;
    correspondence_settings.w_distance_1r      = false;
    correspondence_settings.w_distance_1ddMax  = false;
    correspondence_settings.w_dot_nn           = false;
    correspondence_settings.w_distance_huber   = false;
    correspondence_settings.constr_dir = corresp_dir_ps2mesh;//corresp_dir_mesh2ps;
    //correspondence_settings.constr_dir = corresp_dir_mesh2ps_and_ps2mesh;//corresp_dir_mesh2ps;

    // enable/disable energy terms
    setup_energy_fitting_targets();

    auto& energy_term_list = energy_.energy_term_list();

    auto* pca_tikhonov = energy_term_list.get_term(surface_mesh::ET_TIKHONOV_PCA);
    pca_tikhonov->set_weight(tikhonov_weight);
    pca_tikhonov->set_multiply_factor(1.0);

    auto* pca_lm_term = energy_term_list.get_term(surface_mesh::ET_PCA_LANDMARKS_SKINNED_MULTIPLE_SCANS);
    pca_lm_term->set_weight(use_landmarks_ ? 0.5 : 0.0);
    pca_lm_term->set_multiply_factor(1.0);

    auto* pca_cpc_term = energy_term_list.get_term(surface_mesh::ET_PCA_CPC_PS2MESH_SKINNED_MULTIPLE_SCANS);
    pca_cpc_term->set_weight(use_correspondences_pca_ ? 1.0 : 0.0);
    pca_cpc_term->set_multiply_factor(1.0);

    // load pca model from file
    energy_.energy_term_data().pca.load_pca_model(filename_pca, filename_pca_mapping);
    if (energy_.energy_term_data().pca.dim_pca_model_ == 0)
    {
        std::cerr << "IKPose_fit::start_with_pca: [ERROR] PCA model has dimension 0" << std::endl;
        return false;
    }

    if (energy_.energy_term_data().pca.with_mapping_)
    {
        correspondence_settings.filter_according_to_mapping = true;
        correspondence_settings.mapping =
            &energy_.energy_term_data().pca.mapping_full_to_reduced_;
    }

    // TODO(swenninger): remove pca_mean_character variable, everything should be based off `skin` or `template_character`
    auto* skin = template_character->get_selected_skin();
    auto* pca_mean_character = template_character;

    if (reset_to_pca_mean_)
    {
        // copy positions (in TPose)

        if (energy_.energy_term_data().pca.with_mapping_)
        {
            for (auto v : skin->vertices())
            {
                if (energy_.energy_term_data().pca.mapping_full_to_reduced_[v.idx()] != -1)
                {
                    skin->position(v) = mesh_pca_mean.position(v);
                }
            }
        }
        else
        {
            skin->points() = mesh_pca_mean.points();
        }

        // conformal registration: point set to fit the pca model
        if (true)
        {
            graphene::character::Surface_mesh_skin* skin = character_->get_selected_skin();

            std::vector<Point> src_points;
            std::vector<Point> tar_points;

            if (energy_.energy_term_data().pca.with_mapping_)
            {
                for (auto v : mesh_pca_mean.vertices())
                {
                    if (energy_.energy_term_data().pca.mapping_full_to_reduced_[v.idx()] != -1)
                    {
                        src_points.push_back( skin->unskinned_vertices_[v] );
                        tar_points.push_back( mesh_pca_mean.position(v) );
                    }
                }
            }
            else
            {
                for (auto v : mesh_pca_mean.vertices())
                {
                    src_points.push_back( skin->unskinned_vertices_[v] );
                    tar_points.push_back( mesh_pca_mean.position(v) );
                }
            }

            const Mat4f M = geometry::registration(src_points,
                                                   tar_points,
                                                   graphene::geometry::CONFORMAL_REGISTRATION);

            // transform_mesh(*character_->get_selected_skin(), M);
            // character_->on_shape_changed_shape_in_tpose_but_skeleton_posed(true); // true: with correcting joint positions turned on

            // Also transform point_sets
            for (auto* fitting_target : fitting_targets_)
            {
                if (!transform_points(*(fitting_target->point_set), M))
                {
                    std::cerr << "IKPose_fit::start_with_pca: [ERROR] Can't transform points from point_set_!" << std::endl;
                    return false;
                }
            }
        }

        if (render_callback_)
            render_callback_("Before skeleton pca mean set");


        // pca_mean_character->write_bim("before_full_pca_reset.bim");

        pca_mean_character->on_shape_changed_shape_in_tpose_but_skeleton_posed(true); // true: with correcting joint positions turned on

        // pca_mean_character->write_bim("after_full_pca_reset.bim");

        if (render_callback_)
            render_callback_("Reset to pca mean");


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

        reset_to_pca_mean_ = false;
    }

    // compute normals for pca mean shape
    surface_mesh::crease_normals(skin, 0.0f, "h:normal");

    // set objects for template fitting (energy term data)
    energy_.energy_term_data().template_mesh = pca_mean_character->get_selected_skin();
    energy_.energy_term_data().character     = pca_mean_character;

    // set pca-character for IK pose fit
    if (! set_character(pca_mean_character) )
    {
        std::cerr << "IKPose_fit::start_with_pca: [ERROR] Can't set PCA character" << std::endl;
        return false;
    }

    //
    // @PointSetTransformation
    // NOTE(swenninger):
    // In the first iterations of the PCA + Posefit, the shape of the mesh does not necessarily
    // fit the shape of the pointcloud very well. This can lead to large errors for the
    // registration according to closest point correspondences (usually leading to an unwanted
    // downscaling of the pointcloud).
    // To compensate this, we only allow rigid transformations (=> no scaling) in the
    // closest point correspondences registration.
    // Then, after the first PCA Fit, the shape should already fit the pointcloud well enough and we go back to
    // allowing similarity transformations again.
    //
    do_similarity_transform_ = false;

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

    bool converged = false;
    int       iter = 0;
    do
    {
        //
        // Here, the character should always be in TPose
        //

        //
        // Refine pose for each target without changing the shape of the mesh
        //
        for (size_t fitting_target_index = 0;
             fitting_target_index < num_fitting_targets;
             ++fitting_target_index)
        {
            auto& target = fitting_targets_[fitting_target_index];
            auto& energy_fitting_target = energy_target_data[fitting_target_index];

            // Set character to previously fitted pose
            // pose_vector only consists of rotations

            surface_mesh::set_pose_vector(*energy_fitting_target.pose_vector, pca_mean_character->skeleton());
            pca_mean_character->apply_skinning_on_CPU(true); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals

            if (render_callback_)
                render_callback_("Target pose");

            // Pose Fit (refine previous pose fit, based on landmarks and/or correspondences)
            if (iter > 0)
            {
                if (!start(fitting_target_index))
                {
                    std::cerr << "IKPose_fit::start_with_pca: [ERROR] !start() for fitting target " << fitting_target_index << std::endl;
                    return false;
                }
            }

            // Compute Correspondences in Scanpose
            // they are used for PCA-shape optimization
            if (use_correspondences_pca_)
            {
                if (!target->vertexweighting_property_name.empty())
                {
                    correspondence_settings.vertex_weight_property_name = target->vertexweighting_property_name;
                }
                else
                {
                    correspondence_settings.vertex_weight_property_name = "v:fitting_weight";
                }

                surface_mesh::compute_correspondences(
                        pca_mean_character->get_selected_skin(),
                        energy_fitting_target.point_set,
                        correspondence_settings,
                        energy_fitting_target.correspondences);
            }

            // Save the optimized pose and current IK parameters
            surface_mesh::get_pose_vector(pca_mean_character->skeleton(), *energy_fitting_target.pose_vector);
        }

        // See @PointSetTransformation
        if (iter == 1)
        {
            do_similarity_transform_ = true;
        }

        // Reset skeleton to TPose and skin mesh
        pca_mean_character->skeleton().reset_to_bindpose();
        pca_mean_character->apply_skinning_on_CPU(true); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals

        surface_mesh::set_pose_vector(*fitting_targets_[0]->pose_vector, pca_mean_character->skeleton());

        // optimize joint angles, shape, joint positions, joint angles, shape, joint positions, ..., and joint positions at the end
        if (converged)
        {
            break;
        }

        //
        // Find PCA Parameters that best explain the shape in T-Pose (fits shape)
        //
        energy_.minimize(0.01);
        energy_.energy_term_list().apply_weight_multipliers();

        surface_mesh::crease_normals(pca_mean_character->get_selected_skin(), 0.0f, "h:normal"); // recompute normals for pca mean shape

        // After the PCA Shape-Fit, the joint positions are not correct anymore.
        // As we are in TPose here, it is safe and best to correct joint positions here
        // And copy unskinned properties, since we are in TPose and changed shape
        pca_mean_character->on_shape_changed_shape_in_tpose_but_skeleton_posed(true); // true: with correcting joint positions turned on

        if (render_callback_)
            render_callback_("Posefit: PCA step");

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


        ++iter;

        if (iter >= max_iter)
        {
            converged = true;
        }


    }
    while (/*rel_error > conv_val  &&*/ /*iter < max_iter*/ true );

    // Go back to TPose
    pca_mean_character->skeleton().reset_to_bindpose();
//    pca_mean_character->apply_skinning_on_CPU(true); // true: only selected skin; uses 'unskinned_vertices_', skins them, updates 'vertices_' and normals
//    pca_mean_character->on_shape_changed_in_tpose(true);

    surface_mesh::crease_normals(pca_mean_character->get_selected_skin(), 0.0f, "h:normal"); // recompute normals for pca mean shape

    //
    // Here, the character is in TPose
    //

    //
    // Set skeleton to Scanpose for further processing (non-rigid shape deformation)
    // Note: Don't apply skinning, i.e., the character's geometry is in TPose

    // Reset to first fitting target
    surface_mesh::set_pose_vector(*fitting_targets_[0]->pose_vector, template_character->skeleton());

    with_pca_ = false;

    return true;
}

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

void
IKPose_fit::
copy_pose_parameters_to_target(size_t src_target_index, size_t dst_target_index)
{
    if (src_target_index >= fitting_targets_.size() ||
        dst_target_index >= fitting_targets_.size() ||
        src_target_index == dst_target_index)
    {
        printf("[ERROR] %s: invalid parameters:  src = %lu,   dst = %lu ,  #targets = %lu\n",
               __FUNCTION__, src_target_index, dst_target_index, fitting_targets_.size());
        return;
    }

    std::vector<double> params_src = ik_solvers_[src_target_index]->get_current_parameters();
    if (params_src.size() > 0)
    {
        if (!ik_solvers_[dst_target_index]->set_parameters(params_src))
        {
            std::cerr <<"IKPose_fit::copy_pose_parameters_to_target: [ERROR] Can't copy pose vectors!" << std::endl;
            exit(1);
        }
    }
}

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

bool
IKPose_fit::
minimize(size_t target_index)
{
    do
    {
        if (!minimization_step(target_index))
        {
            std::cerr << "IKPose_fit::minimize: [ERROR] !minimization_step()" << std::endl;
            return false;
        }
    }
    while( !is_converged() );

    std::cout << "Pose fitting with ";
    if (use_landmarks_)
        std::cout << "landmarks";
    if (use_correspondences_ik_ && use_landmarks_)
        std::cout << " and ";
    if (use_correspondences_ik_)
        std::cout << "correspondences";
    std::cout << " done."  << std::endl;

    return true;
}


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


bool
IKPose_fit::
minimization_step(size_t target_index)
{
    if (!is_initialized_ && !init())
    {
        std::cerr << "IKPose_fit::minimization_step: [ERROR] !is_initialized_ && !init()" << std::endl;
        return false;
    }

    if (target_index >= fitting_targets_.size())
    {
        std::cerr << "IKPose_fit::minimization_step: [ERROR] No fitting targets" << std::endl;
        return false;
    }

    using Vertex = surface_mesh::Surface_mesh::Vertex;

    surface_mesh::Surface_mesh* mesh = character_->get_selected_skin();
    auto vpoint = mesh->get_vertex_property<Point>("v:point");

    std::vector<Vec3f> effector_source_positions;
    std::vector<Vec3f> effector_target_positions;
    std::vector<double> effector_weights;

    auto* target = fitting_targets_[target_index];
    auto* ik_solver = ik_solvers_[target_index];

    if (target->no_transform)
    {
        iter_ = max_iterations_ + 1;
        return true;
    }



    // add landmarks to effectors
    surface_mesh::Landmarks_manager landmarks_manager;
    if (use_landmarks_)
    {
        // Get template and pointset landmarks for the specified target
        landmarks_manager.get_landmarks_template_stdvec(mesh, effector_source_positions, target->landmark_property_name);
        landmarks_manager.get_landmarks_point_set_stdvec(target->point_set, effector_target_positions);

        std::vector<Vertex> landmark_vertices;
        landmarks_manager.get_indices_template(mesh, landmark_vertices, target->landmark_property_name);
        auto landmark_weights = mesh->get_vertex_property<double>(target->landmark_weight_property_name);

        double landmark_weight = 1.0;
        if (use_correspondences_ik_)
            landmark_weight = target->landmark_weight_ik;


        // Landmark weight for IK
        for (size_t i = 0; i < effector_source_positions.size(); ++i)
        {
            double local_landmark_weight = 1.0;
            if (landmark_weights)
            {
                local_landmark_weight = landmark_weights[landmark_vertices[i]];
            }

            effector_weights.push_back(local_landmark_weight * landmark_weight);
        }
    }

    // add correspondences to effectors
    ik_correspondences_.clear();
    Correspondences_settings correspondence_settings;
    correspondence_settings.filter_dist        = true;
    correspondence_settings.dist_until         = max_dist_;
    correspondence_settings.filter_normals     = true;
    correspondence_settings.normal_until       = max_normal_angle_;
    correspondence_settings.std_dev_filter     = false;
    correspondence_settings.worst_nPerc_filter = false;
    correspondence_settings.boundary_filter    = true;
    correspondence_settings.symmetry_filter    = false;
    correspondence_settings.w_distance_1r      = false;
    correspondence_settings.w_distance_1ddMax  = false;
    correspondence_settings.w_dot_nn           = false;
    correspondence_settings.w_distance_huber   = false;
    correspondence_settings.constr_dir         = corresp_dir_ps2mesh;//corresp_dir_mesh2ps;
    correspondence_settings.constr_dir         = corresp_dir_mesh2ps_and_ps2mesh;//corresp_dir_mesh2ps;

    if (!target->vertexweighting_property_name.empty())
    {
        correspondence_settings.vertex_weight_property_name = target->vertexweighting_property_name;
    }
    else
    {
        correspondence_settings.vertex_weight_property_name = "v:fitting_weight";
    }

    if (with_pca_)
    {
        if (energy_.energy_term_data().pca.with_mapping_)
        {
            correspondence_settings.filter_according_to_mapping = true;
            correspondence_settings.mapping = &energy_.energy_term_data().pca.mapping_full_to_reduced_;
        }
    }

    if (use_correspondences_ik_)
    {
        // Use either fixed correspondences or compute new ones
        std::vector<Correspondence>* correspondences_to_use = nullptr;
        if (fixed_correspondences_)
        {
            printf("[DEBUG] Using fixed correspondences\n");
            correspondences_to_use = fixed_correspondences_;
        }
        else
        {
            if (surface_mesh::compute_correspondences(mesh, target->point_set,
                                                      correspondence_settings,
                                                      ik_correspondences_))
            {
                correspondences_to_use = &ik_correspondences_;
            }
        }

        if (!correspondences_to_use)
        {
            std::cerr << "[ERROR] Could not determine correspondences" << std::endl;
            return false;
        }

        // Reserve size
        size_t n_correspondences = correspondences_to_use->size();
        effector_source_positions.reserve(effector_source_positions.size() + n_correspondences);
        effector_target_positions.reserve(effector_target_positions.size() + n_correspondences);
        effector_weights.reserve(effector_weights.size() + n_correspondences);

        // Copy correspondence data
        for (size_t i = 0; i < n_correspondences; ++i)
        {
            const Correspondence& c = correspondences_to_use->at(i);

            if (c.constr_dir == corresp_dir_mesh2ps)
            {
                effector_source_positions.push_back(c.on_template);
                effector_target_positions.push_back(c.on_ps);
                effector_weights.push_back(c.weight);
            }
            else
            {
                auto vfc = mesh->vertices(c.face);

                const Point& p0 = vpoint[*vfc];
                const Point& p1 = vpoint[*(++vfc)];
                const Point& p2 = vpoint[*(++vfc)];

                const Vec3f& bc = c.on_template_bc;
                effector_source_positions.push_back(bc[0]*p0 + bc[1]*p1 + bc[2]*p2);

                // effector_source_positions.push_back(c.on_template);
                effector_target_positions.push_back(c.on_ps);
                effector_weights.push_back(c.weight);
            }
        }
    }

    if (effector_target_positions.empty() || effector_source_positions.empty())
    {
        std::cout << "IKPose_fit::minimize: [ERROR] No effector_target_positions and/or effector_source_positions." << std::endl;
        return false;
    }

//    if (render_callback_)
//        render_callback_("Posefit: Min step start state");

    auto registration_method = do_similarity_transform_ ?
                               graphene::geometry::CONFORMAL_REGISTRATION :
                               graphene::geometry::RIGID_REGISTRATION;

    if (!target->optimize_scale)
    {
        registration_method = graphene::geometry::RIGID_REGISTRATION;
    }

    Mat4f M_ps2mesh = geometry::registration(effector_target_positions, effector_source_positions, registration_method);

    if (target->no_transform)
        M_ps2mesh = Mat4f::identity();

    // transform effector targets on point set
    for (size_t i = 0; i < effector_target_positions.size(); ++i)
    {
        effector_target_positions[i] = affine_transform(M_ps2mesh, effector_target_positions[i]);
    }

    // transform the pointset itself
    if (!transform_points(*(target->point_set), M_ps2mesh))
    {
        std::cerr << "IKPose_fit::minimization_step: [ERROR] Can't transform points!" << std::endl;
        return false;
    }

    // NOTE(swenninger):
    //   If we optimize scale for the current target, we want to propagate
    //   the applied scale to all other targets.
    if (target->optimize_scale)
    {
        // Extract scale from transform
        Vec3f scaling(0.0f);
        Mat4f m = M_ps2mesh;
        scaling[0] = norm(Vec3f(m(0,0), m(1, 0), m(2, 0)));
        scaling[1] = norm(Vec3f(m(0,1), m(1, 1), m(2, 1)));
        scaling[2] = norm(Vec3f(m(0,2), m(1, 2), m(2, 2)));
        Mat4f s = Mat4f::scale(scaling);

        // Apply to all other targets whose scale
        // is not optimized independently
        for (size_t i = 0; i < fitting_targets_.size(); ++i)
        {
            if (i == target_index) // Skip current target, scale already applied
                continue;

            if (fitting_targets_[i]->no_transform)
            {
                transform_points(*(fitting_targets_[i]->point_set), m);
            }
            else if (!fitting_targets_[i]->optimize_scale)
            {
                transform_points(*(fitting_targets_[i]->point_set), s);
            }
        }
    }

    if (render_callback_)
        render_callback_("Posefit: Transform step");

    if (!ik_solver->set_targets_and_positions(effector_target_positions, effector_source_positions, effector_weights))
    {
        std::cerr << "IKPose_fit::minimization_step: [ERROR] "
                  << "!ik_solver->set_targets_and_positions(effector_target_positions, effector_source_positions)"
                  << std::endl;
        return false;
    }

    if (!ik_solver->solve_IK())
    {
        std::cerr << "IKPose_fit::minimization_step: [ERROR] !ik_solver->solve_IK()" << std::endl;
        return false;
    }

    // And update global matrices, i.e., recalculate global matrices from updated local
    character_->skeleton().update();

    // Save pose in posevector
    get_pose_vector(character_->skeleton(), *target->pose_vector);

    for (size_t i = 0; i < fitting_targets_.size(); ++i)
    {
        if (i != target_index && fitting_targets_[i]->no_transform)
        {
            fitting_targets_[i]->pose_vector = target->pose_vector;
        }
    }

    character_->apply_skinning_on_CPU(true);

    surface_mesh::crease_normals(character_->get_selected_skin(), 0.0f, "h:normal"); // recompute normals

    if (render_callback_)
        render_callback_("Posefit: minimization step");

    // update position vector with new positions after mesh was skinned
    effector_source_positions.clear();
    effector_source_positions.reserve(effector_target_positions.size());

    if (use_landmarks_)
    {
        landmarks_manager.get_landmarks_template_stdvec(mesh, effector_source_positions, target->landmark_property_name);
    }

    if (use_correspondences_ik_)
    {
        std::vector<Correspondence>* correspondences_to_use = &ik_correspondences_;
        if (fixed_correspondences_)
        {
            correspondences_to_use = fixed_correspondences_;
        }

        for (size_t i = 0; i < correspondences_to_use->size(); ++i)
        {
            const Correspondence& c = correspondences_to_use->at(i);
            if (c.constr_dir == corresp_dir_mesh2ps)
            {
                effector_source_positions.push_back(vpoint[c.on_template_vh]);
            }
            else if (c.constr_dir == corresp_dir_ps2mesh)
            {
                auto vfc = mesh->vertices(c.face);

                const Point& p0 = vpoint[*vfc];
                const Point& p1 = vpoint[*(++vfc)];
                const Point& p2 = vpoint[*(++vfc)];

                const Vec3f& bc = c.on_template_bc;
                effector_source_positions.push_back(bc[0]*p0 + bc[1]*p1 + bc[2]*p2);
            }
        }
    }

    if (effector_target_positions.size() != effector_source_positions.size())
    {
        std::cout << "IKPose_fit::minimize: [ERROR] #effector_target_positions != #effector_source_positions." << std::endl;
        return false;
    }

    error_prev_ = error_;

    error_      = 0.0;
    for (size_t i = 0; i < effector_target_positions.size(); ++i)
    {
        //std::cout << corr_ps[i] << " - " << corr_templ[i] << std::endl;
        const double u = norm(effector_target_positions[i] - effector_source_positions[i]);
        error_ += u*u;
    }
    error_ *= (1.0/effector_target_positions.size());

    rel_error_ = ( std::abs(error_ - error_prev_) / error_prev_ );

    ++iter_;

    printf("Pose fitting iteration %d | E = %.6f | rel. E = %.6f/%f\n", iter_, error_, rel_error_, conv_value_);

    return true;
}


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


bool
IKPose_fit::
transform_points(geometry::Point_set& point_set, const Mat4f& M)
{
    Mat4f& trans = point_set.temp_transformation_;
    trans = M * trans;

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

    // transform full original pointset if available
    for (size_t 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;
    }

    return true;
}


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


bool
IKPose_fit::
load_skinning_info(const std::string& filename_skinning_info_pos,
                   const std::string& filename_joint_positions_chargen,
                   const std::string& filename_skinning_info_wd)
{
    const bool ok_read_mat = graphene::surface_mesh::read_eigen_matrix_from_bin(mat_positions_of_characters_,
                                                                                filename_skinning_info_pos);
    if (!ok_read_mat)
    {
        std::cout << "IKPose_fit::load_skinning_info: [ERROR] !ok_read_mat" << std::endl;
        return false;
    }

    // compute mean character
    mean_character_vec_.resize(mat_positions_of_characters_.rows());
    for (unsigned int i = 0; i < mat_positions_of_characters_.rows(); ++i)
    {
        double value = 0.0;

        for (unsigned int j = 0; j < mat_positions_of_characters_.cols(); ++j)
        {
            value += mat_positions_of_characters_(i, j);
        }
        value /= mat_positions_of_characters_.cols();

        mean_character_vec_(i) = value;
    }

    const bool ok_read_joint_pos_mat = graphene::surface_mesh::read_eigen_matrix_from_bin(mat_joint_pos_of_characters_,
                                                                                          filename_joint_positions_chargen);
    if (!ok_read_joint_pos_mat)
    {
        std::cerr << "IKPose_fit::load_skinning_info: [ERROR] !ok_read_joint_pos_mat" << std::endl;
        return false;
    }

    // LLS stuff
    Eigen::MatrixXd AtA = mat_positions_of_characters_.transpose() * mat_positions_of_characters_;
    solver_.compute(AtA);
    if(solver_.info() != Eigen::Success)
    {
        // decomposition failed
        std::cout << "IKPose_fit::load_skinning_info: [ERROR] Decomposition failed." << std::endl;
        return false;
    }

    // weights and dependencies
    std::ifstream ifs_skinning_info( filename_skinning_info_wd, std::ofstream::binary );
    if (!ifs_skinning_info.is_open())
    {
        std::cout << "IKPose_fit::load_skinning_info: [ERROR] !ifs_skinning_info.is_open()." << std::endl;
        return false;
    }
    unsigned int dim_1_skinning_info = 0; // n_mesh_vertices
    unsigned int dim_2_skinning_info = 0; // n_bims
    ifs_skinning_info.read(reinterpret_cast<char *>(&dim_1_skinning_info), sizeof(dim_1_skinning_info));
    ifs_skinning_info.read(reinterpret_cast<char *>(&dim_2_skinning_info), sizeof(dim_2_skinning_info));
    unsigned int n_mesh_vertices = dim_1_skinning_info;
    unsigned int n_bims          = dim_2_skinning_info;

    skinning_info_.resize( n_mesh_vertices );
    for (unsigned int i = 0; i < n_mesh_vertices; ++i)
    {
        skinning_info_[i].resize(n_bims);
    }

    for (unsigned int j = 0; j < n_bims; ++j)
    {
        for (unsigned int i = 0; i < n_mesh_vertices; ++i)
        {
            unsigned int dim_3_pairs = 0;
            ifs_skinning_info.read(reinterpret_cast<char *>(&dim_3_pairs), sizeof(dim_3_pairs));
            skinning_info_[i][j].resize(dim_3_pairs);
            for (unsigned int k = 0; k < dim_3_pairs; ++k)
            {
                uint8_t first  = 0;
                float   second = 0.0f;
                ifs_skinning_info.read(reinterpret_cast<char *>(&first), sizeof(first));
                ifs_skinning_info.read(reinterpret_cast<char *>(&second), sizeof(second));

                std::pair< uint8_t, float > tmp_pair = std::make_pair( first, second );

                skinning_info_[i][j][k] = tmp_pair;
            }
        }
    }
    ifs_skinning_info.close();

    return true;
}


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


void
IKPose_fit::
update_skinning_weights_wrt_mix(Surface_mesh* current_mesh)
{
    Surface_mesh mesh_copy = *current_mesh;

    // conformal registration from copied current mesh to mean character
    std::vector<Point> src_points;
    std::vector<Point> tar_points;
    for ( auto v : mesh_copy.vertices() )
    {
        const int idx = v.idx();

        const Point pos_mean_character(mean_character_vec_[3*idx+0],
                                       mean_character_vec_[3*idx+1],
                                       mean_character_vec_[3*idx+2]);

        src_points.push_back( mesh_copy.position(v) );
        tar_points.push_back( pos_mean_character );
    }

    const Mat4f M = geometry::registration(src_points,
                                           tar_points,
                                           graphene::geometry::CONFORMAL_REGISTRATION);

    transform_mesh(mesh_copy, M);

    // rhs
    Eigen::VectorXd rhs(3*mesh_copy.n_vertices());
    for (auto v : mesh_copy.vertices())
    {
        const int idx = v.idx();

        const Point& p = mesh_copy.position(v);

        rhs(3*idx + 0) = p[0];
        rhs(3*idx + 1) = p[1];
        rhs(3*idx + 2) = p[2];
    }
    Eigen::VectorXd Atb = mat_positions_of_characters_.transpose() * rhs;

    // solve LLS
    Eigen::VectorXd x_solution = solver_.solve(Atb);
    if(solver_.info() != Eigen::Success)
    {
        // solving failed
        std::cout << "IKPose_fit::update_skinning_weights_wrt_mix: [ERROR] Solving failed." << std::endl;
        exit(1);
    }

    const bool use_quadratic_programming = true;
    if (use_quadratic_programming)
    {
        Eigen::MatrixXd AtA = mat_positions_of_characters_.transpose() * mat_positions_of_characters_;

        const int n = AtA.rows();

        // quadratic programming s.t. a_ij in [0 , 1]
        // matrix A
        alglib::real_2d_array alglib_A;
        alglib_A.setlength(n, n);
        for (unsigned int i = 0; i < n; ++i)
        {
            for (unsigned int j = 0; j < n; ++j)
            {
                alglib_A(i, j) = AtA(i, j);
            }
        }
        // vector b
        alglib::real_1d_array alglib_b;
        alglib_b.setlength(n);
        for (unsigned int i = 0; i < n; ++i)
        {
            alglib_b(i) = -Atb(i);
        }
        // vector x0 - starting point
        alglib::real_1d_array alglib_x0;
        alglib_x0.setlength(n);
        for (unsigned int i = 0; i < n; ++i)
        {
            alglib_x0(i) = x_solution(i);
        }
        // scale
        alglib::real_1d_array alglib_s;
        alglib_s.setlength(n);
        for (unsigned int i = 0; i < n; ++i)
        {
            alglib_s(i) = 1.0;
        }
        // lower bound
        alglib::real_1d_array alglib_bndl;
        alglib_bndl.setlength(n);
        for (unsigned int i = 0; i < n; ++i)
        {
            alglib_bndl(i) = 0.0;
        }
        // upper bound
        alglib::real_1d_array alglib_bndu;
        alglib_bndu.setlength(n);
        for (unsigned int i = 0; i < n; ++i)
        {
            alglib_bndu(i) = 1.0;
        }
        // create solver, set quadratic/linear terms
        alglib::real_1d_array alglib_x; // alglib solution vector
        alglib::minqpstate alglib_state;
        alglib::minqpreport alglib_rep;
        minqpcreate(n, alglib_state);
        minqpsetquadraticterm(alglib_state, alglib_A);
        minqpsetlinearterm(alglib_state, alglib_b);
        minqpsetstartingpoint(alglib_state, alglib_x0);
        minqpsetbc(alglib_state, alglib_bndl, alglib_bndu);
        // Set scale of the parameters.
        minqpsetscale(alglib_state, alglib_s);
        // solve problem with BLEIC-based QP solver
        // default stopping criteria are used.
        alglib::minqpsetalgobleic(alglib_state, 0.0, 0.0, 0.0, 0);
        alglib::minqpoptimize(alglib_state);
        alglib::minqpresults(alglib_state, alglib_x, alglib_rep);
        for (unsigned int i = 0; i < n; ++i)
        {
            x_solution(i) = alglib_x(i);
        }
    }

    /*
    // DEBUGGING
    std::cerr << "[DEBUG] Solution: " << std::endl << x_solution << std::endl;
    double residuum = (mat_positions_of_characters_*x_solution - rhs).squaredNorm();
    std::cerr << "[DEBUG] ERROR: " << residuum << std::endl;
    double sum = 0;
    for (unsigned int i = 0; i < x_solution.rows(); ++i)
    {
        sum += x_solution(i);
    }
    std::cerr << "sum: " << sum << std::endl;
    exit(1);
    */

    // actually update skinning weights
    auto cm_v_weights  = current_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    auto cm_v_weights2 = current_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    auto cm_v_depends  = current_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    auto cm_v_depends2 = current_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    if (!cm_v_weights || !cm_v_weights2 || !cm_v_depends || !cm_v_depends)
    {
        std::cerr << "[DEBUG] IKPose_fit::update_skinning_weights_wrt_mix(...) - !cm_v_weights || !cm_v_weights2 || !cm_v_depends || !cm_v_depends" << std::endl;
        exit(1);
    }
    for (auto v : current_mesh->vertices())
    {
        const int idx = v.idx();

        // collect pairs
        std::unordered_map< uint8_t, float > all_pairs_map;
        for (unsigned int i = 0; i < x_solution.rows(); ++i)
        {
            if (x_solution(i) > 0)
            {
                for (unsigned int k = 0; k < skinning_info_[idx][i].size(); ++k)
                {
                    uint8_t joint_idx = skinning_info_[idx][i][k].first;
                    float   weight    = skinning_info_[idx][i][k].second;

                    if (all_pairs_map.count(joint_idx) == 0)
                    {
                        all_pairs_map[joint_idx] = x_solution(i) * weight;
                    }
                    else
                    {
                        all_pairs_map[joint_idx] += x_solution(i) * weight;
                    }
                }
            }
        }

        // find biggest pairs
        std::vector<std::pair<float, uint8_t>> all_pairs_vec_reverse;
        for (std::pair<uint8_t, float> current_pair : all_pairs_map)
        {
            std::pair< float, uint8_t > tmp_pair = std::make_pair( current_pair.second, current_pair.first );
            all_pairs_vec_reverse.push_back(tmp_pair);
        }
        std::sort(all_pairs_vec_reverse.begin(), all_pairs_vec_reverse.end());
        std::reverse(all_pairs_vec_reverse.begin(), all_pairs_vec_reverse.end());

        Vec4f best_v_weights(0.0f);
        Vec4f best_v_weights2(0.0f);
        Vec4f best_v_depends(0.0f);
        Vec4f best_v_depends2(0.0f);

        // 1
        if (all_pairs_vec_reverse.size() >= 1)
        {
            best_v_depends[0] = all_pairs_vec_reverse[0].second;
            best_v_weights[0] = all_pairs_vec_reverse[0].first;
        }
        // 2
        if (all_pairs_vec_reverse.size() >= 2)
        {
            best_v_depends[1] = all_pairs_vec_reverse[1].second;
            best_v_weights[1] = all_pairs_vec_reverse[1].first;
        }
        // 3
        if (all_pairs_vec_reverse.size() >= 3)
        {
            best_v_depends[2] = all_pairs_vec_reverse[2].second;
            best_v_weights[2] = all_pairs_vec_reverse[2].first;
        }
        // 4
        if (all_pairs_vec_reverse.size() >= 4)
        {
            best_v_depends[3] = all_pairs_vec_reverse[3].second;
            best_v_weights[3] = all_pairs_vec_reverse[3].first;
        }
        // 5
        if (all_pairs_vec_reverse.size() >= 5)
        {
            best_v_depends2[0] = all_pairs_vec_reverse[4].second;
            best_v_weights2[0] = all_pairs_vec_reverse[4].first;
        }
        // 6
        if (all_pairs_vec_reverse.size() >= 6)
        {
            best_v_depends2[1] = all_pairs_vec_reverse[5].second;
            best_v_weights2[1] = all_pairs_vec_reverse[5].first;
        }
        // 7
        if (all_pairs_vec_reverse.size() >= 7)
        {
            best_v_depends2[2] = all_pairs_vec_reverse[6].second;
            best_v_weights2[2] = all_pairs_vec_reverse[6].first;
        }
        // 8
        if (all_pairs_vec_reverse.size() >= 8)
        {
            best_v_depends2[3] = all_pairs_vec_reverse[7].second;
            best_v_weights2[3] = all_pairs_vec_reverse[7].first;
        }
        const float total_sum_weights = best_v_weights[0] + best_v_weights[1]+
                                        best_v_weights[2] + best_v_weights[3]+
                                        best_v_weights2[0] + best_v_weights2[1]+
                                        best_v_weights2[2] + best_v_weights2[3];
        best_v_weights  /= total_sum_weights;
        best_v_weights2 /= total_sum_weights;

        cm_v_weights[v]  = best_v_weights;
        cm_v_weights2[v] = best_v_weights2;
        cm_v_depends[v]  = best_v_depends;
        cm_v_depends2[v] = best_v_depends2;
    }
}


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


int
IKPose_fit::
update_skinning_weights_wrt_nearest(Surface_mesh* current_mesh)
{
    Surface_mesh mesh_copy = *current_mesh;

    // find closest character
    int    idx_closest_character = -1;
    double     current_min_error = DBL_MAX;
    for (unsigned int j = 0; j < mat_positions_of_characters_.cols(); ++j)
    {
        // conformal registration from copied current mesh to mean character
        std::vector<Point> src_points;
        std::vector<Point> tar_points;
        for ( auto v : mesh_copy.vertices() )
        {
            const int idx = v.idx();

            const Point pos_chargen_character(mat_positions_of_characters_(3*idx+0, j),
                                              mat_positions_of_characters_(3*idx+1, j),
                                              mat_positions_of_characters_(3*idx+2, j));

            src_points.push_back( mesh_copy.position(v) );
            tar_points.push_back( pos_chargen_character );
        }

        const Mat4f M = geometry::registration(src_points,
                                               tar_points,
                                               graphene::geometry::CONFORMAL_REGISTRATION);

        transform_mesh(mesh_copy, M);

        // compute error between registered copy of current mesh and character from Character Generator
        double current_error = 0.0;
        for (auto v : mesh_copy.vertices())
        {
            const int idx = v.idx();

            const Point pos_current_character(mat_positions_of_characters_(3*idx+0, j),
                                              mat_positions_of_characters_(3*idx+1, j),
                                              mat_positions_of_characters_(3*idx+2, j));

            const double dist = distance(mesh_copy.position(v), pos_current_character);

            current_error += dist*dist;
        }
        current_error /= mesh_copy.n_vertices();

        if (current_error < current_min_error)
        {
            idx_closest_character = j;
            current_min_error = current_error;
        }
    }

    if (idx_closest_character == -1)
    {
        return -1;
    }

    // actually update skinning weights
    auto cm_v_weights  = current_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    auto cm_v_weights2 = current_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    auto cm_v_depends  = current_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    auto cm_v_depends2 = current_mesh->get_vertex_property<Vec4f>("v:skin_depend2");

    if (!cm_v_weights || !cm_v_weights2 || !cm_v_depends || !cm_v_depends)
    {
        std::cerr << "[DEBUG] IKPose_fit::update_skinning_weights_wrt_nearest(...) - !cm_v_weights || !cm_v_weights2 || !cm_v_depends || !cm_v_depends" << std::endl;
        exit(1);
    }
    for ( auto v : current_mesh->vertices() )
    {
        const int idx = v.idx();

        Vec4f best_v_weights(0.0f);
        Vec4f best_v_weights2(0.0f);
        Vec4f best_v_depends(0.0f);
        Vec4f best_v_depends2(0.0f);

        // 1
        if (skinning_info_[idx][idx_closest_character].size() >= 1)
        {
            best_v_depends[0] = skinning_info_[idx][idx_closest_character][0].first;
            best_v_weights[0] = skinning_info_[idx][idx_closest_character][0].second;
        }
        // 2
        if (skinning_info_[idx][idx_closest_character].size() >= 2)
        {
            best_v_depends[1] = skinning_info_[idx][idx_closest_character][1].first;
            best_v_weights[1] = skinning_info_[idx][idx_closest_character][1].second;
        }
        // 3
        if (skinning_info_[idx][idx_closest_character].size() >= 3)
        {
            best_v_depends[2] = skinning_info_[idx][idx_closest_character][2].first;
            best_v_weights[2] = skinning_info_[idx][idx_closest_character][2].second;
        }
        // 4
        if (skinning_info_[idx][idx_closest_character].size() >= 4)
        {
            best_v_depends[3] = skinning_info_[idx][idx_closest_character][3].first;
            best_v_weights[3] = skinning_info_[idx][idx_closest_character][3].second;
        }
        // 5
        if (skinning_info_[idx][idx_closest_character].size() >= 5)
        {
            best_v_depends2[0] = skinning_info_[idx][idx_closest_character][4].first;
            best_v_weights2[0] = skinning_info_[idx][idx_closest_character][4].second;
        }
        // 6
        if (skinning_info_[idx][idx_closest_character].size() >= 6)
        {
            best_v_depends2[1] = skinning_info_[idx][idx_closest_character][5].first;
            best_v_weights2[1] = skinning_info_[idx][idx_closest_character][5].second;
        }
        // 7
        if (skinning_info_[idx][idx_closest_character].size() >= 7)
        {
            best_v_depends2[2] = skinning_info_[idx][idx_closest_character][6].first;
            best_v_weights2[2] = skinning_info_[idx][idx_closest_character][6].second;
        }
        // 8
        if (skinning_info_[idx][idx_closest_character].size() >= 8)
        {
            best_v_depends2[3] = skinning_info_[idx][idx_closest_character][7].first;
            best_v_weights2[3] = skinning_info_[idx][idx_closest_character][7].second;
        }

        cm_v_weights[v]  = best_v_weights;
        cm_v_weights2[v] = best_v_weights2;
        cm_v_depends[v]  = best_v_depends;
        cm_v_depends2[v] = best_v_depends2;
    }

    return idx_closest_character;
}


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


int
IKPose_fit::
update_skinning_weights_wrt_hardcoded_character_generator_model(Surface_mesh* current_mesh)
{
    int idx_closest_character = 29;

    // actually update skinning weights
    auto cm_v_weights  = current_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    auto cm_v_weights2 = current_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    auto cm_v_depends  = current_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    auto cm_v_depends2 = current_mesh->get_vertex_property<Vec4f>("v:skin_depend2");

    if (!cm_v_weights || !cm_v_weights2 || !cm_v_depends || !cm_v_depends)
    {
        std::cerr << "[DEBUG] IKPose_fit::update_skinning_weights_wrt_nearest(...) - !cm_v_weights || !cm_v_weights2 || !cm_v_depends || !cm_v_depends" << std::endl;
        exit(1);
    }
    for ( auto v : current_mesh->vertices() )
    {
        const int idx = v.idx();

        Vec4f best_v_weights(0.0f);
        Vec4f best_v_weights2(0.0f);
        Vec4f best_v_depends(0.0f);
        Vec4f best_v_depends2(0.0f);

        // 1
        if (skinning_info_[idx][idx_closest_character].size() >= 1)
        {
            best_v_depends[0] = skinning_info_[idx][idx_closest_character][0].first;
            best_v_weights[0] = skinning_info_[idx][idx_closest_character][0].second;
        }
        // 2
        if (skinning_info_[idx][idx_closest_character].size() >= 2)
        {
            best_v_depends[1] = skinning_info_[idx][idx_closest_character][1].first;
            best_v_weights[1] = skinning_info_[idx][idx_closest_character][1].second;
        }
        // 3
        if (skinning_info_[idx][idx_closest_character].size() >= 3)
        {
            best_v_depends[2] = skinning_info_[idx][idx_closest_character][2].first;
            best_v_weights[2] = skinning_info_[idx][idx_closest_character][2].second;
        }
        // 4
        if (skinning_info_[idx][idx_closest_character].size() >= 4)
        {
            best_v_depends[3] = skinning_info_[idx][idx_closest_character][3].first;
            best_v_weights[3] = skinning_info_[idx][idx_closest_character][3].second;
        }
        // 5
        if (skinning_info_[idx][idx_closest_character].size() >= 5)
        {
            best_v_depends2[0] = skinning_info_[idx][idx_closest_character][4].first;
            best_v_weights2[0] = skinning_info_[idx][idx_closest_character][4].second;
        }
        // 6
        if (skinning_info_[idx][idx_closest_character].size() >= 6)
        {
            best_v_depends2[1] = skinning_info_[idx][idx_closest_character][5].first;
            best_v_weights2[1] = skinning_info_[idx][idx_closest_character][5].second;
        }
        // 7
        if (skinning_info_[idx][idx_closest_character].size() >= 7)
        {
            best_v_depends2[2] = skinning_info_[idx][idx_closest_character][6].first;
            best_v_weights2[2] = skinning_info_[idx][idx_closest_character][6].second;
        }
        // 8
        if (skinning_info_[idx][idx_closest_character].size() >= 8)
        {
            best_v_depends2[3] = skinning_info_[idx][idx_closest_character][7].first;
            best_v_weights2[3] = skinning_info_[idx][idx_closest_character][7].second;
        }

        cm_v_weights[v]  = best_v_weights;
        cm_v_weights2[v] = best_v_weights2;
        cm_v_depends[v]  = best_v_depends;
        cm_v_depends2[v] = best_v_depends2;
    }

    return idx_closest_character;
}


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


bool
IKPose_fit::
update_joint_position_MVCs_from_chargen(Surface_mesh* current_mesh, int idx_closest_character)
{
    Surface_mesh mesh_copy = *current_mesh;

    // find closest character
    int tmp_idx_closest_character = -1;
    if (idx_closest_character >= 0)
    {
        tmp_idx_closest_character = idx_closest_character;
    }
    else
    {
        double     current_min_error = DBL_MAX;
        for (unsigned int j = 0; j < mat_positions_of_characters_.cols(); ++j)
        {
            // conformal registration from copied current mesh to mean character
            std::vector<Point> src_points;
            std::vector<Point> tar_points;
            for ( auto v : mesh_copy.vertices() )
            {
                const int idx = v.idx();

                const Point pos_chargen_character(mat_positions_of_characters_(3*idx+0, j),
                                                  mat_positions_of_characters_(3*idx+1, j),
                                                  mat_positions_of_characters_(3*idx+2, j));

                src_points.push_back( mesh_copy.position(v) );
                tar_points.push_back( pos_chargen_character );
            }

            const Mat4f M = geometry::registration(src_points,
                                                   tar_points,
                                                   graphene::geometry::CONFORMAL_REGISTRATION);

            transform_mesh(mesh_copy, M);

            // compute error between registered copy of current mesh and character from Character Generator
            double current_error = 0.0;
            for (auto v : mesh_copy.vertices())
            {
                const int idx = v.idx();

                const Point pos_current_character(mat_positions_of_characters_(3*idx+0, j),
                                                  mat_positions_of_characters_(3*idx+1, j),
                                                  mat_positions_of_characters_(3*idx+2, j));

                const double dist = distance(mesh_copy.position(v), pos_current_character);

                current_error += dist*dist;
            }
            current_error /= mesh_copy.n_vertices();

            if (current_error < current_min_error)
            {
                tmp_idx_closest_character = j;
                current_min_error = current_error;
            }
        }
    }

    // compute similarity transformation from closest character generator mesh to current mesh
    std::vector<Point> src_points;
    std::vector<Point> tar_points;
    for ( auto v : mesh_copy.vertices() )
    {
        const int idx = v.idx();

        const Point pos_closest_character(mat_positions_of_characters_(3*idx+0, tmp_idx_closest_character),
                                          mat_positions_of_characters_(3*idx+1, tmp_idx_closest_character),
                                          mat_positions_of_characters_(3*idx+2, tmp_idx_closest_character));

        src_points.push_back( pos_closest_character );
        tar_points.push_back( current_mesh->position(v) );
    }
    const Mat4f M_closest2current = geometry::registration(src_points,
                                                           tar_points,
                                                           graphene::geometry::CONFORMAL_REGISTRATION);

    // set vertex positions
    for (auto v : mesh_copy.vertices())
    {
        const int idx = v.idx();

        const Point pos_closest_character(mat_positions_of_characters_(3*idx+0, tmp_idx_closest_character),
                                          mat_positions_of_characters_(3*idx+1, tmp_idx_closest_character),
                                          mat_positions_of_characters_(3*idx+2, tmp_idx_closest_character));

        mesh_copy.position(v) = pos_closest_character;
    }
    mesh_copy.update_face_normals();
    mesh_copy.update_vertex_normals();

    // transform vertex positions
    transform_mesh(mesh_copy, M_closest2current);

    // and joint positions
    std::vector<Point> joint_points;
    unsigned int n_joints = mat_joint_pos_of_characters_.rows() / 3;
    joint_points.resize(n_joints);
    for (unsigned int i = 0; i < n_joints; ++i)
    {
        const Point joint_pos(mat_joint_pos_of_characters_(3*i+0, tmp_idx_closest_character),
                              mat_joint_pos_of_characters_(3*i+1, tmp_idx_closest_character),
                              mat_joint_pos_of_characters_(3*i+2, tmp_idx_closest_character));

        joint_points[i] = affine_transform(M_closest2current, joint_pos);
    }

    // update mean value coordinates
    surface_mesh::mvc::compute_mean_value_coords(&mesh_copy, joint_points, "m:mvc_joints");

    // and copy
    auto mvc_tmp_src = mesh_copy.get_mesh_property<std::vector<std::vector<float>>>("m:mvc_joints");
    auto mvc_tmp_tar = current_mesh->get_mesh_property<std::vector<std::vector<float>>>("m:mvc_joints");
    if (!mvc_tmp_src || !mvc_tmp_tar)
    {
        std::cerr << "[DEBUG] IKPose_fit::update_joint_position_MVCs_from_chargen(...) - !mvc_tmp_src || !mvc_tmp_tar" << std::endl;
        return false;
    }

    mvc_tmp_tar[0] = mvc_tmp_src[0];

    return true;
}


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