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


#include "Energy_term_collisions_tri_vs_p.h"

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

#include "../settings.h"


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


namespace graphene {
namespace surface_mesh {


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


Energy_term_collisions_tri_vs_p::
Energy_term_collisions_tri_vs_p(const Energy_term_data& energy_term_data, double weight)
: Energy_term(energy_term_data, weight, "Collisions_tri_vs_p")
{
}


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


Energy_term_collisions_tri_vs_p::
~Energy_term_collisions_tri_vs_p()
{}


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


double
Energy_term_collisions_tri_vs_p::
evaluate()
{
    const std::unordered_map<int, Point >& correspondences = energy_term_data_.collisions_tri_vs_p;
    if (correspondences.size() == 0) // in case there are no collision
    {
        return 0.0;
    }

    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

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

    auto mesh_vertices_collision_weight_tri_vs_p = template_mesh->get_vertex_property<double>("v:collision_weight_tri_vs_p");
    if (!mesh_vertices_collision_weight_tri_vs_p)
    {
        std::cerr << "[ERROR] Energy_term_collisions_tri_vs_p::evaluate() - !mesh_vertices_collision_weight_tri_vs_p" << std::endl;
        exit(1);
    }

    double error_match_poi2poi = 0.0;
    double sum_weights         = 0.0;
    for (auto corr : correspondences)
    {
        double u = norm( template_points[Surface_mesh::Vertex(corr.first)] - corr.second );
        if ( robustness_parameters_.solver_type == SOLVER_TYPE_L2 )
        {
            error_match_poi2poi += mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)] * u * u;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_L1 )
        {
            error_match_poi2poi += mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)] * 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 += mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)] * ( huber_thr*u - huber_thr*huber_thr/2.0 );
            }
            else
            {
                error_match_poi2poi += mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)] * ( u*u / 2.0 );
            }
        }
        else // default to L2
        {
            error_match_poi2poi += mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)] * u * u;
        }

        sum_weights += mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)];
    }
    error_match_poi2poi *= weight_ / correspondences.size();

    return error_match_poi2poi;
}


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


void
Energy_term_collisions_tri_vs_p::
add_rows_small_lse( std::vector<Tripl>& coeffs,
                    Eigen::MatrixXd& B,
                    unsigned int& r)
{
    //std::cout << "[INFO] Energy_term_collisions_tri_vs_p::add_rows_small_lse(): weight_: " << weight_ << std::endl;

    const Column_offsets& column_offsets = energy_term_data_.column_offsets;
    const std::unordered_map<int, Point >& correspondences = energy_term_data_.collisions_tri_vs_p;
    const unsigned int offset_match = column_offsets.offset_match;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    auto mesh_vertices_collision_weight_tri_vs_p = template_mesh->get_vertex_property<double>("v:collision_weight_tri_vs_p");
    if (!mesh_vertices_collision_weight_tri_vs_p)
    {
        std::cerr << "[ERROR] Energy_term_collisions_tri_vs_p::add_rows_small_lse() - !mesh_vertices_collision_weight_tri_vs_p" << std::endl;
        exit(1);
    }

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

    const double match_weight = sqrt(weight_ / correspondences.size());
    for (auto corr : correspondences)
    {
        const double cWeight = sqrt(mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)]);

        // right hand side
        for (unsigned int j = 0; j < 3; ++j)
        {
            B(r, j) = match_weight * cWeight * corr.second[j];
        }

        // begin_row
        coeffs.push_back( Tripl(r, offset_match + corr.first, cWeight * match_weight) );
        ++r;
    }
}


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


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

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

    const Column_offsets& column_offsets = energy_term_data_.column_offsets;
    const std::unordered_map<int, Point >& correspondences = energy_term_data_.collisions_tri_vs_p;
    const unsigned int offset_match = column_offsets.offset_match;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;
  
    auto mesh_vertices_collision_weight_tri_vs_p = template_mesh->get_vertex_property<double>("v:collision_weight_tri_vs_p");
    if (!mesh_vertices_collision_weight_tri_vs_p)
    {
        std::cerr << "[ERROR] Energy_term_collisions_tri_vs_p::add_rows_big_lse() - !mesh_vertices_collision_weight_tri_vs_p" << std::endl;
        exit(1);
    }

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

    const double weight_match_poi2poi = sqrt( weight_ / correspondences.size() );
    for (auto corr : correspondences)
    {
        const double cWeight = sqrt(mesh_vertices_collision_weight_tri_vs_p[Surface_mesh::Vertex(corr.first)]);

        // get closest point
        const Point& cp = corr.second;

        // right hand side
        b(r) = cWeight * weight_match_poi2poi * cp[0];

        // begin_row
        coeffs.push_back( Tripl(r, offset_match + 3*corr.first, cWeight * weight_match_poi2poi) ); // z_i
        ++r;

        // right hand side
        b(r) = cWeight * weight_match_poi2poi * cp[1];

        // begin_row
        coeffs.push_back( Tripl(r, offset_match + 3*corr.first + 1, cWeight * weight_match_poi2poi) ); // z_i
        ++r;

        // right hand side
        b(r) = cWeight * weight_match_poi2poi * cp[2];

        // begin_row
        coeffs.push_back( Tripl(r, offset_match + 3*corr.first + 2, cWeight * weight_match_poi2poi) ); // z_i
        ++r;
    }
}

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


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

    return (unsigned int)energy_term_data_.collisions_tri_vs_p.size();
}

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


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

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


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