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


#include "Energy_term_ears_skinned.h"

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

#include "../settings.h"


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


namespace graphene {
namespace surface_mesh {


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


Energy_term_ears_skinned::
Energy_term_ears_skinned(const Energy_term_data &energy_term_data, double weight) :
    Energy_term(energy_term_data, weight, "Ears Skinned")
{}


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


Energy_term_ears_skinned::
~Energy_term_ears_skinned()
{}


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


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

    auto vdepends  = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    auto vweights  = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    auto vdepends2 = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    auto vweights2 = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");

    if (!(vdepends && vweights && vdepends2 && vweights2))
    {
        std::cerr << "[ERROR] - Energy_term_ears_skinned::pre_minimize_action() - EXIT!" << std::endl;
        exit(1);
    }

    auto& target = energy_term_data_.ear_fitting_target;

    if(!target.point_set || !target.pose_vector)
    {
        std::cerr << "[ERROR] - Energy_term_ears_skinned - energy_term_data_.ear_fitting_target not setup correctly!" << std::endl;
        exit(1);
    }
}


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


double
Energy_term_ears_skinned::
evaluate()
{
    auto& target = energy_term_data_.ear_fitting_target;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;
    const Point_set* point_set = target.point_set;

    auto vdepends  = template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    auto vweights  = template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    auto vdepends2 = template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    auto vweights2 = template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");

    // Set pose and get skinning matrices
    surface_mesh::set_pose_vector(*target.pose_vector, energy_term_data_.character->skeleton());
    std::vector<Mat4f> skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;

    const auto ears_lm_prop =
        template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >(target.landmark_property_name);
    const std::vector<Surface_mesh::Vertex>& ears_lm  = ears_lm_prop[0];

    const std::vector<unsigned int>& ears_targets_idx = point_set->lm_ears_;

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

    double error_match_ears = 0.0;
    const unsigned int L = ears_targets_idx.size();
    for (unsigned int i = 0; i < L; ++i)
    {
        auto v = ears_lm[i];
        Mat4f blend_mat = blend_matrices(vweights[v], vweights2[v],
                                         vdepends[v], vdepends2[v],
                                         skinning_tpose_to_scanpose);

        auto skinned_template_point = affine_transform(blend_mat, template_points[v]);
        double u = norm( point_set->points_[ears_targets_idx[i]] - skinned_template_point );
        error_match_ears += u * u;
    }
    error_match_ears *= (weight_ / L);

    return error_match_ears;
}


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


void
Energy_term_ears_skinned::
add_rows_big_lse(std::vector<Tripl> &coeffs,
                 Eigen::VectorXd &b,
                 unsigned int &r)
{
    //std::cout << "[INFO] Energy_term_ears_skinned::add_rows_big_lse(): weight_: " << weight_ << std::endl;
    auto& target = energy_term_data_.ear_fitting_target;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;
    const Point_set* point_set = target.point_set;

    auto vdepends  = template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    auto vweights  = template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    auto vdepends2 = template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    auto vweights2 = template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");

    const auto ears_lm_prop
        = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:ears_lm");
    const std::vector<Surface_mesh::Vertex>& ears_lm  = ears_lm_prop[0];

    const std::vector<unsigned int>& ears_targets_idx = point_set->lm_ears_;

    // Set pose and get skinning matrices
    surface_mesh::set_pose_vector(*target.pose_vector, energy_term_data_.character->skeleton());
    std::vector<Mat4f> skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;

    const Column_offsets& column_offsets = energy_term_data_.column_offsets;

    const unsigned int L = ears_targets_idx.size();

    const double weight_match_ears = sqrt(weight_ / L);

    //
    // NOTE(swenninger): Skinned ear landmark on mesh should be at pointset landmark
    //
    //   skin(i) = blend_mat_i * {pos_i, 1} = blend_mat_i_3x3 * pos_i + blend_mat_i_trans
    //
    //   blend_mat_i_3x3 * pos_i + blend_mat_i_trans === point_set[i]
    //   blend_mat_i_3x3 * pos_i                     === point_set[i] - blend_mat_i_trans
    //

    for (unsigned int i = 0; i < L; ++i)
    {
        auto v = ears_lm[i];
        const int idx = ears_lm[i].idx();
        const Point t = point_set->points_[ears_targets_idx[i]];

        Mat4f blend_mat = blend_matrices(vweights[v], vweights2[v],
                                         vdepends[v], vdepends2[v],
                                         skinning_tpose_to_scanpose);

        // right hand side
        b(r) = weight_match_ears * (t[0] - blend_mat(0, 3));

        // begin_row
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 0, weight_match_ears * blend_mat(0, 0)) );
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 1, weight_match_ears * blend_mat(0, 1)) );
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 2, weight_match_ears * blend_mat(0, 2)) );
        ++r;

        // right hand side
        b(r) = weight_match_ears * (t[1] - blend_mat(1, 3));

        // begin_row
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 0, weight_match_ears * blend_mat(1, 0)) );
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 1, weight_match_ears * blend_mat(1, 1)) );
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 2, weight_match_ears * blend_mat(1, 2)) );
        ++r;

        // right hand side
        b(r) = weight_match_ears * (t[2] - blend_mat(2, 3));

        // begin_row
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 0, weight_match_ears * blend_mat(2, 0)) );
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 1, weight_match_ears * blend_mat(2, 1)) );
        coeffs.push_back( Tripl(r, column_offsets.offset_match + 3*idx + 2, weight_match_ears * blend_mat(2, 2)) );
        ++r;
    }
}


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


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

    const auto& target = energy_term_data_.ear_fitting_target;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;
    const Point_set*        point_set = target.point_set;

    const std::vector<unsigned int>& ears_targets_idx = point_set->lm_ears_;

    const unsigned int L = ears_targets_idx.size();

    return 3*L;
}


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