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


#include "Energy_term_point_to_plane_skinned.h"

#include <graphene/geometry/bary_coord.h>
#include <graphene/geometry/Blend_skinning_matrices.h>

#include <iostream>
#include <fstream>

#include "../settings.h"


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


namespace graphene {
namespace surface_mesh {


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


Energy_term_point_to_plane_skinned::
Energy_term_point_to_plane_skinned(const Energy_term_data& energy_term_data, double weight)
: Energy_term(energy_term_data, weight, "Point to Plane Skinned")
{
}


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


Energy_term_point_to_plane_skinned::
~Energy_term_point_to_plane_skinned()
{}


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


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

    const std::vector<Mat4f>&  skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;

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

    // TODO: check if property exists?
    auto template_points              = template_mesh->get_vertex_property<Point>("v:point");

    static int iter = 0;
    std::ofstream ofs;

    if (debug_enabled_)
    {
        ofs.open(std::to_string(iter) + "_p2plane_debug.txt");
        iter++;
    }

    double error_match_poi2pla = 0.0;
    double sum_weights         = 0.0;
    for (auto corr : correspondences)
    {
        if ( corr.constr_dir == corresp_dir_mesh2ps )
        {
            std::cerr << "Mesh2Pointset Correspondence direction not implemented for Point_to_plane_skinned Energy" << std::endl;
            exit(-1);
            //error_match_poi2pla += corr.weight * std::pow( dot( corr.on_ps_n , template_points[corr.on_template_vh] - corr.on_ps) , 2 );
        }
        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& bc = corr.on_template_bc;

            Mat4f blend_mat_v0 = blend_matrices(vweights1[v0], vweights2[v0],
                                                vdepends1[v0], vdepends2[v0],
                                                skinning_tpose_to_scanpose);
            Mat4f blend_mat_v1 = blend_matrices(vweights1[v1], vweights2[v1],
                                                vdepends1[v1], vdepends2[v1],
                                                skinning_tpose_to_scanpose);
            Mat4f blend_mat_v2 = blend_matrices(vweights1[v2], vweights2[v2],
                                                vdepends1[v2], vdepends2[v2],
                                                skinning_tpose_to_scanpose);

            Point p0 = affine_transform(blend_mat_v0, template_points[v0]);
            Point p1 = affine_transform(blend_mat_v1, template_points[v1]);
            Point p2 = affine_transform(blend_mat_v2, template_points[v2]);

            Point skinned_pos = bc[0] * p0 + bc[1] * p1 + bc[2] * p2;

            if (debug_enabled_)
            {
                ofs << skinned_pos << " 200 20 20 1.0 0.4 0.4" << std::endl;
                ofs << corr.on_ps << " 20 200 20 1.0 0.4 0.4" << std::endl;
            }

            error_match_poi2pla += corr.weight * std::pow( dot( corr.on_ps_n , skinned_pos - corr.on_ps) , 2 );
        }
        sum_weights += corr.weight;
    }
    error_match_poi2pla *= (weight_/ sum_weights); // TODO VLLT VON ENERGYTERMDATA NEHMEN ?!

    return error_match_poi2pla;
}


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


void
Energy_term_point_to_plane_skinned::
add_rows_big_lse( std::vector<Tripl>& coeffs,
                  Eigen::VectorXd& b,
                  unsigned int& r)
{
    const Column_offsets& column_offsets = energy_term_data_.column_offsets;
    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;

    const std::vector<Mat4f>&  skinning_tpose_to_scanpose = energy_term_data_.character->skeleton().skinning_matrices_;

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


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

    const double weight_match_poi2pla = 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;

        // get normal of closest point
        const Normal& cpn = correspondences[i].on_ps_n;
//        const Normal& cpn = correspondences[i].on_template_n;

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

        if ( correspondences[i].constr_dir == corresp_dir_mesh2ps )
        {
            std::cerr << "Mesh2Pointset Correspondence direction not implemented for Point_to_plane_skinned Energy" << std::endl;
            exit(-1);

            // // right hand side
            // b(r) = cWeight * weight_match_poi2pla * dot(cpn, cp);

            // // get vertex handle of sample
            // const Surface_mesh::Vertex& vh = correspondences[i].on_template_vh;

            // // begin_row
            // coeffs.push_back( Tripl(r, offset_match + 3*vh.idx()    , cWeight * weight_match_poi2pla * cpn[0]) ); // z_i
            // coeffs.push_back( Tripl(r, offset_match + 3*vh.idx() + 1, cWeight * weight_match_poi2pla * cpn[1]) ); // z_i
            // coeffs.push_back( Tripl(r, offset_match + 3*vh.idx() + 2, cWeight * weight_match_poi2pla * cpn[2]) ); // z_i
            // ++r;
        }
        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;

            Mat4f blend_mat_v0 = blend_matrices(vweights1[v0], vweights2[v0],
                                                vdepends1[v0], vdepends2[v0],
                                                skinning_tpose_to_scanpose);
            Mat4f blend_mat_v1 = blend_matrices(vweights1[v1], vweights2[v1],
                                                vdepends1[v1], vdepends2[v1],
                                                skinning_tpose_to_scanpose);
            Mat4f blend_mat_v2 = blend_matrices(vweights1[v2], vweights2[v2],
                                                vdepends1[v2], vdepends2[v2],
                                                skinning_tpose_to_scanpose);

            // Extract columns for each coordinate for each vertex from blend_mats
            Vec3f r0v0 = Vec3f(blend_mat_v0(0,0), blend_mat_v0(1,0), blend_mat_v0(2,0));
            Vec3f r1v0 = Vec3f(blend_mat_v0(0,1), blend_mat_v0(1,1), blend_mat_v0(2,1));
            Vec3f r2v0 = Vec3f(blend_mat_v0(0,2), blend_mat_v0(1,2), blend_mat_v0(2,2));

            Vec3f r0v1 = Vec3f(blend_mat_v1(0,0), blend_mat_v1(1,0), blend_mat_v1(2,0));
            Vec3f r1v1 = Vec3f(blend_mat_v1(0,1), blend_mat_v1(1,1), blend_mat_v1(2,1));
            Vec3f r2v1 = Vec3f(blend_mat_v1(0,2), blend_mat_v1(1,2), blend_mat_v1(2,2));

            Vec3f r0v2 = Vec3f(blend_mat_v2(0,0), blend_mat_v2(1,0), blend_mat_v2(2,0));
            Vec3f r1v2 = Vec3f(blend_mat_v2(0,1), blend_mat_v2(1,1), blend_mat_v2(2,1));
            Vec3f r2v2 = Vec3f(blend_mat_v2(0,2), blend_mat_v2(1,2), blend_mat_v2(2,2));

            // Extract translations for each vertex from blend_mats
            Vec3f t_v0 = Vec3f(blend_mat_v0(0,3), blend_mat_v0(1,3), blend_mat_v0(2,3));
            Vec3f t_v1 = Vec3f(blend_mat_v1(0,3), blend_mat_v1(1,3), blend_mat_v1(2,3));
            Vec3f t_v2 = Vec3f(blend_mat_v2(0,3), blend_mat_v2(1,3), blend_mat_v2(2,3));

            // begin_row
            // Right hand side
            b(r) = cWeight * weight_match_poi2pla * dot(cpn, cp - bc[0] * t_v0 - bc[1] * t_v1 - bc[2] * t_v2);

            // Left hand side
            coeffs.push_back( Tripl(r, offset_match + 3 * v0.idx() + 0, cWeight * weight_match_poi2pla * bc[0] * dot(cpn, r0v0)) );
            coeffs.push_back( Tripl(r, offset_match + 3 * v0.idx() + 1, cWeight * weight_match_poi2pla * bc[0] * dot(cpn, r1v0)) );
            coeffs.push_back( Tripl(r, offset_match + 3 * v0.idx() + 2, cWeight * weight_match_poi2pla * bc[0] * dot(cpn, r2v0)) );

            coeffs.push_back( Tripl(r, offset_match + 3 * v1.idx() + 0, cWeight * weight_match_poi2pla * bc[1] * dot(cpn, r0v1)) );
            coeffs.push_back( Tripl(r, offset_match + 3 * v1.idx() + 1, cWeight * weight_match_poi2pla * bc[1] * dot(cpn, r1v1)) );
            coeffs.push_back( Tripl(r, offset_match + 3 * v1.idx() + 2, cWeight * weight_match_poi2pla * bc[1] * dot(cpn, r2v1)) );

            coeffs.push_back( Tripl(r, offset_match + 3 * v2.idx() + 0, cWeight * weight_match_poi2pla * bc[2] * dot(cpn, r0v2)) );
            coeffs.push_back( Tripl(r, offset_match + 3 * v2.idx() + 1, cWeight * weight_match_poi2pla * bc[2] * dot(cpn, r1v2)) );
            coeffs.push_back( Tripl(r, offset_match + 3 * v2.idx() + 2, cWeight * weight_match_poi2pla * bc[2] * dot(cpn, r2v2)) );
            ++r;
        }
    }
}

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


unsigned int
Energy_term_point_to_plane_skinned::
n_rows_big_sle()
{
    return (unsigned int)energy_term_data_.correspondences.size();
}


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