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


#include "Energy_term_point_to_point_skinned.h"

#include <graphene/geometry/bary_coord.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_point_to_point_skinned::
Energy_term_point_to_point_skinned(const Energy_term_data& energy_term_data, double weight)
: Energy_term(energy_term_data, weight, "Point to Point Skinned")
{
}


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


Energy_term_point_to_point_skinned::
~Energy_term_point_to_point_skinned()
{}


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

void
Energy_term_point_to_point_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_point_to_point_skinned::pre_minimize_action() - EXIT!" << std::endl;
        exit(1);
    }
}

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


double
Energy_term_point_to_point_skinned::
evaluate()
{
    const std::vector<Correspondence>& correspondences = energy_term_data_.correspondences;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

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

    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_point_to_point_skinned::evaluate() - EXIT!" << std::endl;
        exit(1);
    }

    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::ofstream ofs;
    static int iter = 0;
    if (debug_enabled_)
    {
        std::string fn_debug_file = "e_p2p_skinned_" + std::to_string(iter) + ".txt";
        ofs.open(fn_debug_file);
        iter++;
    }

    double error_match_poi2poi = 0.0;
    double sum_weights         = 0.0;
    for (auto corr : correspondences)
    {
        if ( corr.constr_dir == corresp_dir_mesh2ps )
        {
            std::cerr << "[ERROR] in Energy_term_point_to_point_skinned::evaluate(...)' - mesh2ps not supported!" << std::endl;
            exit(1);
        }
        else if ( corr.constr_dir == corresp_dir_ps2mesh )
        {
            auto fvit = template_mesh->vertices(corr.face);
            const auto v0 = *fvit;
            const auto v1 = *(++fvit);
            const auto v2 = *(++fvit);
            // get barycentric coordinates
            const Point& b = corr.on_template_bc;

            Mat4f blend_mat_v0;
            Mat4f blend_mat_v1;
            Mat4f blend_mat_v2;

            if (energy_term_data_.use_dualquaternion_skinning)
            {
                blend_mat_v0 = blend_dqs(vweights[v0], vweights2[v0],
                                         vdepends[v0], vdepends2[v0], dqs);
                blend_mat_v1 = blend_dqs(vweights[v1], vweights2[v1],
                                         vdepends[v1], vdepends2[v1], dqs);
                blend_mat_v2 = blend_dqs(vweights[v2], vweights2[v2],
                                         vdepends[v2], vdepends2[v2], dqs);
            }
            else
            {
                blend_mat_v0 = blend_matrices(vweights[v0], vweights2[v0],
                                              vdepends[v0], vdepends2[v0],
                                              skinning_tpose_to_scanpose);
                blend_mat_v1 = blend_matrices(vweights[v1], vweights2[v1],
                                              vdepends[v1], vdepends2[v1],
                                              skinning_tpose_to_scanpose);
                blend_mat_v2 = blend_matrices(vweights[v2], vweights2[v2],
                                              vdepends[v2], vdepends2[v2],
                                              skinning_tpose_to_scanpose);
            }

            Point the_pointA = affine_transform(blend_mat_v0, template_points[v0]);
            Point the_pointB = affine_transform(blend_mat_v1, template_points[v1]);
            Point the_pointC = affine_transform(blend_mat_v2, template_points[v2]);

            Point new_x_pos = b[0]*the_pointA + b[1]*the_pointB + b[2]*the_pointC;

            if (debug_enabled_)
            {
                ofs << new_x_pos  << " 200 20 30 0.1 0.4 0.5" << std::endl;
                ofs << corr.on_ps << " 20 200 30 0.1 0.4 0.5" << std::endl;
            }

            double u = norm( new_x_pos - corr.on_ps );
            if ( robustness_parameters_.solver_type == SOLVER_TYPE_L2 )
            {
                error_match_poi2poi += corr.weight * u * u;
            }
            else if ( robustness_parameters_.solver_type == SOLVER_TYPE_L1 )
            {
                error_match_poi2poi += corr.weight * std::pow( u, robustness_parameters_.parameter );
            }
            else if ( robustness_parameters_.solver_type == SOLVER_TYPE_HUBER )
            {
                double huber_thr = robustness_parameters_.parameter;
                if ( u > huber_thr )
                {
                    error_match_poi2poi += corr.weight * ( huber_thr*u - huber_thr*huber_thr/2.0 );
                }
                else
                {
                    error_match_poi2poi += corr.weight * ( u*u/2.0 );
                }
            }
/*
            else if ( robustness_parameters_.solver_type == SOLVER_TYPE_GEMANMCCLURE )
            {
                double scale_parameter = robustness_parameters_.parameter;
//                    error_match_poi2poi += corr.weight * ( u*u / (scale_parameter*scale_parameter + u*u) ); // VERSION 2
                error_match_poi2poi += corr.weight * ( ( u*u / ( 2.0 * ( 1.0 + (u*u/(sigma_hat*sigma_hat * scale_parameter*scale_parameter)) ) ) ) );
            }
*/
            else // default to L2
            {
                error_match_poi2poi += corr.weight * u * u;
            }
        }
        sum_weights         += corr.weight;
    }
    error_match_poi2poi *= (weight_ / sum_weights); // TODO evtl. sum_weights aus energy_term_data nehmen ?!

    return error_match_poi2poi;
}


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


void
Energy_term_point_to_point_skinned::
add_rows_big_lse( std::vector<Tripl>& coeffs,
                  Eigen::VectorXd& b,
                  unsigned int& r)

{
    //std::cout << "[INFO] Energy_term_point_to_point_skinned::add_rows_big_lse(): weight_: " << weight_ << std::endl;

    const Column_offsets& column_offsets = energy_term_data_.column_offsets;
    //choose either the skinnend correspondences or the "normal" ones based on whether skinned correspondences exist or not
    const std::vector<Correspondence>& correspondences = energy_term_data_.correspondences;
    const unsigned int offset_match = column_offsets.offset_match;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

#ifdef BE_VERBOSE
    std::cerr << "in: 'Energy_term_point_to_point_skinned::add_rows_big_lgs(...)'" << std::endl;
#endif


    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_point_to_point_skinned::add_rows_big_lse() - EXIT!" << std::endl;
        exit(1);
    }

    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]);
        }
    }

    const double weight_match_poi2poi = sqrt( weight_ / energy_term_data_.sum_corresp_weights );
    for (unsigned int i = 0; i < correspondences.size(); ++i)
    {
        // get closest point
        const Point& cp = correspondences[i].on_ps;

        const double cWeight = sqrt(correspondences[i].weight);

        if ( correspondences[i].constr_dir == corresp_dir_mesh2ps )
        {
            std::cerr << "[ERROR] in Energy_term_point_to_point_skinned::add_rows_big_lse(...)' - mesh2ps not supported!" << std::endl;
            exit(1);
        }
        else if ( correspondences[i].constr_dir == corresp_dir_ps2mesh )
        {
            // get vertices of closest triangle
            auto fvit = template_mesh->vertices(correspondences[i].face);
            const auto v0 = *fvit;
            const auto v1 = *(++fvit);
            const auto v2 = *(++fvit);

            // get barycentric coordinates
            const Point& bc = correspondences[i].on_template_bc;

            int idxA = v0.idx();
            int idxB = v1.idx();
            int idxC = v2.idx();

            Mat4f blend_mat_v0;
            Mat4f blend_mat_v1;
            Mat4f blend_mat_v2;

            if (energy_term_data_.use_dualquaternion_skinning)
            {
                blend_mat_v0 = blend_dqs(vweights[v0], vweights2[v0],
                                         vdepends[v0], vdepends2[v0], dqs);
                blend_mat_v1 = blend_dqs(vweights[v1], vweights2[v1],
                                         vdepends[v1], vdepends2[v1], dqs);
                blend_mat_v2 = blend_dqs(vweights[v2], vweights2[v2],
                                         vdepends[v2], vdepends2[v2], dqs);
            }
            else
            {
                blend_mat_v0 = blend_matrices(vweights[v0], vweights2[v0],
                                              vdepends[v0], vdepends2[v0],
                                              skinning_tpose_to_scanpose);
                blend_mat_v1 = blend_matrices(vweights[v1], vweights2[v1],
                                              vdepends[v1], vdepends2[v1],
                                              skinning_tpose_to_scanpose);
                blend_mat_v2 = blend_matrices(vweights[v2], vweights2[v2],
                                              vdepends[v2], vdepends2[v2],
                                              skinning_tpose_to_scanpose);
            }

            // right hand side
            b(r) = cWeight * weight_match_poi2poi * (cp[0] - bc[0]*blend_mat_v0(0, 3) - bc[1]*blend_mat_v1(0, 3) - bc[2]*blend_mat_v2(0, 3));

            // begin_row
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 0, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(0, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 1, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(0, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 2, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(0, 2)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 0, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(0, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 1, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(0, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 2, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(0, 2)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 0, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(0, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 1, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(0, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 2, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(0, 2)) );
            ++r;

            // right hand side
            b(r) = cWeight * weight_match_poi2poi * (cp[1] - bc[0]*blend_mat_v0(1, 3) - bc[1]*blend_mat_v1(1, 3) - bc[2]*blend_mat_v2(1, 3));

            // begin_row
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 0, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(1, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 1, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(1, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 2, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(1, 2)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 0, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(1, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 1, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(1, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 2, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(1, 2)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 0, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(1, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 1, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(1, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 2, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(1, 2)) );
            ++r;

            // right hand side
            b(r) = cWeight * weight_match_poi2poi * (cp[2] - bc[0]*blend_mat_v0(2, 3) - bc[1]*blend_mat_v1(2, 3) - bc[2]*blend_mat_v2(2, 3));

            // begin_row
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 0, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(2, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 1, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(2, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxA + 2, cWeight * weight_match_poi2poi * bc[0]*blend_mat_v0(2, 2)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 0, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(2, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 1, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(2, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxB + 2, cWeight * weight_match_poi2poi * bc[1]*blend_mat_v1(2, 2)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 0, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(2, 0)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 1, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(2, 1)) );
            coeffs.push_back( Tripl(r, offset_match + 3*idxC + 2, cWeight * weight_match_poi2poi * bc[2]*blend_mat_v2(2, 2)) );
            ++r;
        }
    }
}


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


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

    return (unsigned int)3*energy_term_data_.correspondences.size();
}


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