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


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

#include "../settings.h"


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


namespace graphene {
namespace surface_mesh {


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


Energy_term_landmarks_skinned::
Energy_term_landmarks_skinned(const Energy_term_data& energy_term_data, double weight) :
    Energy_term(energy_term_data, weight, "Landmarks Skinned")
{
}


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


Energy_term_landmarks_skinned::
~Energy_term_landmarks_skinned()
{}

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

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

    const std::vector<Mat4f>&  skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;
    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) || skinning_tpose_to_scanpose.size() == 0)
    {
        std::cerr << "[ERROR] - Energy_term_landmarks_skinned::pre_minimize_action() - EXIT!" << std::endl;
        exit(1);
    }
}

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

double
Energy_term_landmarks_skinned::
evaluate()
{
    const Landmarks_manager& landmarks_manager = energy_term_data_.landmarks_manager;
    const Point_set*    point_set     = energy_term_data_.point_set;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    const std::string& lm_prop_name = energy_term_data_.landmark_property_name;
    const std::string& lm_weight_prop_name = energy_term_data_.landmark_weight_property_name;

    const std::vector<Mat4f>&  skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;
    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");
    if (!(vdepends && vweights && vdepends2 && vweights2))
    {
        std::cerr << "[ERROR] - Energy_term_landmarks_skinned::evaluate() - EXIT!" << std::endl;
        exit(1);
    }

    if (!landmarks_manager.check_dimension(point_set, template_mesh, lm_prop_name))
    {
        std::cerr << "[ERROR] - Energy_term_landmarks_skinned::evaluate() - No landmarks!" << std::endl;
        return DBL_MAX;
    }

    std::vector<DQuatf> dqs;
    if (energy_term_data_.use_dualquaternion_skinning)
    {
        dqs.resize(skinning_tpose_to_scanpose.size());
        for (size_t i = 0; i < dqs.size(); ++i)
        {
            mat4_to_dualquat(skinning_tpose_to_scanpose[i], dqs[i]);
        }
    }

    std::vector<Point> lm_ps;
    landmarks_manager.get_landmarks_point_set_stdvec(point_set, lm_ps);
    std::vector<Point> lm_templ;
    landmarks_manager.get_landmarks_template_stdvec(template_mesh, lm_templ, lm_prop_name);
    std::vector<Surface_mesh::Vertex> tm_landmarks;
    landmarks_manager.get_indices_template(template_mesh, tm_landmarks, lm_prop_name);

    Surface_mesh::Vertex_property<double> local_landmark_weight = template_mesh->get_vertex_property<double>(lm_weight_prop_name);
    const std::vector<Surface_mesh::Vertex>& landmark_prop = template_mesh->get_mesh_property<std::vector<Surface_mesh::Vertex> >(lm_prop_name)[0];

    std::ofstream ofs;
    static int iter = 0;
    if (debug_enabled_)
    {
        std::string fn_debug_file = "debug_lm_skinned_" + std::to_string(iter) + ".txt";
        ofs.open(fn_debug_file);
        iter++;
    }

    double error_match_landmarks = 0.0;
    double local_weight = 1.0;
    const unsigned int L = lm_ps.size();
    for (unsigned int i = 0; i < L; ++i)
    {
        if (local_landmark_weight)
        {
            local_weight = local_landmark_weight[landmark_prop[i]];
            if (local_weight < 1e-5)
                continue;
            local_weight = sqrt(local_weight);
        }

        const int idx = tm_landmarks[i].idx();

        Surface_mesh::Vertex v_idx(idx);

        Mat4f blend_mat;

        if (energy_term_data_.use_dualquaternion_skinning)
        {
            blend_mat = blend_dqs(vweights[v_idx], vweights2[v_idx],
                                  vdepends[v_idx], vdepends2[v_idx],
                                  dqs);
        }
        else
        {
            blend_mat = blend_matrices(vweights[v_idx], vweights2[v_idx],
                                       vdepends[v_idx], vdepends2[v_idx],
                                       skinning_tpose_to_scanpose);
        }

        Point the_point = affine_transform(blend_mat, lm_templ[i]);

        if (debug_enabled_)
        {
                ofs << the_point << " 200 20 30 0.1 0.4 0.5" << std::endl;
                ofs << lm_ps[i]  << " 20 200 30 0.1 0.4 0.5" << std::endl;
        }

        double u = norm( lm_ps[i] - the_point );
        if ( robustness_parameters_.solver_type == SOLVER_TYPE_L2 )
        {
            error_match_landmarks += u * u * local_weight;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_L1 )
        {
            error_match_landmarks += std::pow( u, robustness_parameters_.parameter ) * local_weight;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_HUBER )
        {
            std::cerr << "ERSTMAL NICHT " << std::endl; exit(1);
            double huber_thr = robustness_parameters_.parameter;
            if ( u > huber_thr )
            {
                error_match_landmarks += ( huber_thr*u - huber_thr*huber_thr/2.0 ) * local_weight;
            }
            else
            {
                error_match_landmarks += ( u*u/2.0 ) * local_weight;
            }
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_GEMANMCCLURE )
        {
            std::cerr << "ERSTMAL NICHT " << std::endl; exit(1);
            double scale_parameter = robustness_parameters_.parameter;
//                error_match_landmarks += ( u*u / (scale_parameter*scale_parameter + u*u) ); // VERSION 2
            error_match_landmarks += ( ( u*u / ( 2.0 * ( 1.0 + (u*u/(scale_parameter*scale_parameter)) ) ) ) ) * local_weight; // VERSION 3
        }
        else // default to L2
        {
            error_match_landmarks += u * u * local_weight;
        }
    }
    error_match_landmarks *= (weight_ / L);

    return error_match_landmarks;
}


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


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

    const Landmarks_manager& landmarks_manager = energy_term_data_.landmarks_manager;
    const Column_offsets &column_offsets = energy_term_data_.column_offsets;
    const Point_set* point_set = energy_term_data_.point_set;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    const std::string& lm_prop_name = energy_term_data_.landmark_property_name;
    const std::string& lm_weight_prop_name = energy_term_data_.landmark_weight_property_name;

    const std::vector<Mat4f>&  skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;
    Surface_mesh::Vertex_property<Vec4f> vdepends = template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    Surface_mesh::Vertex_property<Vec4f> vweights = template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    Surface_mesh::Vertex_property<Vec4f> vdepends2 = template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    Surface_mesh::Vertex_property<Vec4f> vweights2 = template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    if (!(vdepends && vweights && vdepends2 && vweights2))
    {
        std::cerr << "[ERROR] - Energy_term_landmarks_skinned::add_rows_big_lse() - EXIT!" << std::endl;
        exit(1);
    }

    if (!landmarks_manager.check_dimension(point_set, template_mesh, lm_prop_name))
    {
        std::cerr << "[ERROR] - Energy_term_landmarks_skinned::add_rows_big_lse() - No landmarks!" << std::endl;
        return;
    }

    std::vector<DQuatf> dqs;
    if (energy_term_data_.use_dualquaternion_skinning)
    {
        dqs.resize(skinning_tpose_to_scanpose.size());
        for (size_t i = 0; i < dqs.size(); ++i)
        {
            mat4_to_dualquat(skinning_tpose_to_scanpose[i], dqs[i]);
        }
    }

    double local_weight = 1.0;
    Surface_mesh::Vertex_property<double> local_landmark_weight = template_mesh->get_vertex_property<double>(lm_weight_prop_name);

    std::vector<Surface_mesh::Vertex> tm_landmarks;
    landmarks_manager.get_indices_template(template_mesh, tm_landmarks, lm_prop_name);
    std::vector<Point> lm_ps;
    landmarks_manager.get_landmarks_point_set_stdvec(point_set, lm_ps);

    static int iter = 0;
    std::ofstream debug_ofs;
    if (debug_enabled_)
    {
        std::string fn_debug_file = "debug_energy_setup_lm_skinned_" + std::to_string(iter) + ".txt";
        debug_ofs.open(fn_debug_file);
        ++iter;
    }

    const unsigned int L = lm_ps.size();

    const double weight_match_landmarks = sqrt(weight_ / L);
    for (unsigned int i = 0; i < L; ++i)
    {
        const int idx = tm_landmarks[i].idx();
        const Point t = lm_ps[i];

        if (local_landmark_weight)
        {
            local_weight = local_landmark_weight[tm_landmarks[i]];
            if (local_weight < 1e-5)
            {
                continue;
            }
            local_weight = sqrt(local_weight);
        }

        Surface_mesh::Vertex v_idx(idx);

        Mat4f blend_mat;

        if (energy_term_data_.use_dualquaternion_skinning)
        {
            blend_mat = blend_dqs(vweights[v_idx], vweights2[v_idx],
                                  vdepends[v_idx], vdepends2[v_idx],
                                  dqs);
        }
        else
        {
            blend_mat = blend_matrices(vweights[v_idx], vweights2[v_idx],
                                       vdepends[v_idx], vdepends2[v_idx],
                                       skinning_tpose_to_scanpose);
        }

        if (debug_enabled_)
        {
            auto p_unskinned = template_mesh->position(v_idx);
            auto p = blend_mat * Vec4f(p_unskinned[0], p_unskinned[1], p_unskinned[2], 1.0);

            debug_ofs << Vec3f(p[0], p[1], p[2]) << " 200 20 30 0.1 0.4 0.5" << std::endl;
            debug_ofs << t << " 20 200 30 0.1 0.4 0.5" << std::endl;
        }

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

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

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

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

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

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


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


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

    const std::string& lm_prop_name = energy_term_data_.landmark_property_name;
    const std::string& lm_weight_prop_name = energy_term_data_.landmark_weight_property_name;

    unsigned int c=0;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;
    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > lm_prop = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >(lm_prop_name);
    if (lm_prop)
    {
        const std::vector<Surface_mesh::Vertex>& tm_landmarks = lm_prop[0];
        Surface_mesh::Vertex_property<double> local_landmark_weight = template_mesh->get_vertex_property<double>(lm_weight_prop_name);

        if (local_landmark_weight)
        {
            for (size_t i=0; i < tm_landmarks.size(); ++i)
            {
                if (local_landmark_weight[tm_landmarks[i]] >= 1e-5)
                {
                    ++c;
                }
                else
                {
                    //std::cout << "landmark ignored " << tm_landmarks[i] << std::endl;
                }
            }
        }
        else
        {
            c = (unsigned int) tm_landmarks.size();
        }
    }

    return c*3;
}



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