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


#include "Energy_term_pca_boundary.h"
//#include <graphene/surface_mesh/algorithms/templatefitting/utility/my_helper.h>

#include "../settings.h"


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


namespace graphene {
namespace surface_mesh {


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


Energy_term_pca_boundary::
Energy_term_pca_boundary(const Energy_term_data& energy_term_data, double weight)
: Energy_term(energy_term_data, weight, "PCA Boundary")
{
}


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


Energy_term_pca_boundary::
~Energy_term_pca_boundary()
{}


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


double
Energy_term_pca_boundary::
evaluate()
{
    const PCA& pca = energy_term_data_.pca;
    const Point_set* point_set = energy_term_data_.point_set;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > headmodel_landmarks_boundary
        = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:mesh_boundary_landmarks");
    Surface_mesh::Mesh_property< std::vector<Point> > target_landmarks_boundary
        = template_mesh->get_mesh_property< std::vector<Point> >("m:target_boundary_landmarks");

    if (!headmodel_landmarks_boundary || !target_landmarks_boundary)
    {
        std::cerr << "Energy_term_pca_boundary::evaluate(): [ERROR] !headmodel_landmarks_boundary || !target_landmarks_boundary." << std::endl;
        return DBL_MAX;
    }

    const std::vector<Surface_mesh::Vertex>& headmodel_landmarks_boundary_vec = headmodel_landmarks_boundary[0];
    const std::vector<Point>&                target_landmarks_boundary_vec    = target_landmarks_boundary[0];

    // check dimensions
    if (headmodel_landmarks_boundary_vec.size() != target_landmarks_boundary_vec.size())
    {
        std::cerr << "Energy_term_pca_boundary::evaluate(): [ERROR] Number of landmarks at boundary do not match." << std::endl;
        return DBL_MAX;
    }

    // transform target points w.r.t. 'temp_transformation_'
    const Mat4f M_ps_trans = point_set->temp_transformation_;
    std::vector<Point> trans_target_landmarks_boundary_vec = target_landmarks_boundary_vec;
    for (unsigned int i = 0; i < target_landmarks_boundary_vec.size(); ++i)
    {
        Point p = target_landmarks_boundary_vec[i];
        p = affine_transform(M_ps_trans, p);
        trans_target_landmarks_boundary_vec[i] = p;
    }

    double error_fit_pca_boundary = 0.0;

    const unsigned int           L(target_landmarks_boundary_vec.size());

    for (unsigned int i = 0; i < L; ++i)
    {
        const int idx = headmodel_landmarks_boundary_vec[i].idx();

        double mi_x = pca.m_pca_(3*idx    );
        double mi_y = pca.m_pca_(3*idx + 1);
        double mi_z = pca.m_pca_(3*idx + 2);
        const Point  mi(mi_x, mi_y, mi_z); // TODO EVTL. SCHNELLER ?!

        Point  Pid(0.0, 0.0, 0.0);
        for (unsigned int j = 0; j < 3; ++j)
        {
            for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
            {
                Pid[j] += pca.P_pca_(3*idx + j, dcount) * pca.d_pca_(dcount);
            }
        }

        double u = norm( Pid + mi - trans_target_landmarks_boundary_vec[i] );
        if ( robustness_parameters_.solver_type == SOLVER_TYPE_L2 )
        {
            error_fit_pca_boundary += u * u;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_L1 )
        {
            error_fit_pca_boundary += 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_fit_pca_boundary += ( huber_thr*u - huber_thr*huber_thr/2.0 );
            }
            else
            {
                error_fit_pca_boundary += ( u*u/2.0 );
            }
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_GEMANMCCLURE )
        {
            double scale_parameter = robustness_parameters_.parameter;
//                error_reg_pca_ff += ( u*u / (scale_parameter*scale_parameter + u*u) ); // VERSION 2
              error_fit_pca_boundary += ( ( u*u / ( 2.0 * ( 1.0 + (u*u/(scale_parameter*scale_parameter)) ) ) ) ); // VERSION 3
        }
        else // default to L2
        {
            error_fit_pca_boundary += u * u;
        }
    }
    error_fit_pca_boundary *= (weight_ / L);

    return error_fit_pca_boundary;
}


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


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

    const Column_offsets& column_offsets = energy_term_data_.column_offsets;
    const unsigned int offset_nrd_w_pca = column_offsets.offset_nrd_w_pca;
    const PCA& pca = energy_term_data_.pca;
    const geometry::Point_set* point_set = energy_term_data_.point_set;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > headmodel_landmarks_boundary
        = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:mesh_boundary_landmarks");
    Surface_mesh::Mesh_property< std::vector<Point> > target_landmarks_boundary
        = template_mesh->get_mesh_property< std::vector<Point> >("m:target_boundary_landmarks");

    if (!headmodel_landmarks_boundary || !target_landmarks_boundary)
    {
        std::cerr << "Energy_term_pca_boundary::add_rows_big_lse(): [ERROR] !headmodel_landmarks_boundary || !target_landmarks_boundary." << std::endl;
        return;
    }

    const std::vector<Surface_mesh::Vertex>& headmodel_landmarks_boundary_vec = headmodel_landmarks_boundary[0];
    const std::vector<Point>&                target_landmarks_boundary_vec    = target_landmarks_boundary[0];

    // check dimensions
    if (headmodel_landmarks_boundary_vec.size() != target_landmarks_boundary_vec.size())
    {
        std::cerr << "Energy_term_pca_boundary::add_rows_big_lse(): [ERROR] Number of landmarks at boundary do not match." << std::endl;
        return;
    }

    // transform target points w.r.t. 'temp_transformation_'
    const Mat4f M_ps_trans = point_set->temp_transformation_;
    std::vector<Point> trans_target_landmarks_boundary_vec = target_landmarks_boundary_vec;
    for (unsigned int i = 0; i < target_landmarks_boundary_vec.size(); ++i)
    {
        Point p = target_landmarks_boundary_vec[i];
        p = affine_transform(M_ps_trans, p);
        trans_target_landmarks_boundary_vec[i] = p;
    }

    const unsigned int           L(target_landmarks_boundary_vec.size());

    // landmarks at boundary
    const std::vector<Point>& landmarks_pointset = trans_target_landmarks_boundary_vec;

    const double weight_model = sqrt(weight_ / L);
    //std::cout << "[INFO] Energy_term_pca_boundary::add_rows_big_lse(): weight_model: " << weight_model << std::endl;
    for (unsigned int i = 0; i < L; ++i)
    {
        int idx = headmodel_landmarks_boundary_vec[i].idx();

        // right hand side
        b(r) = weight_model * ( landmarks_pointset[i][0] - pca.m_pca_(3*idx + 0) );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, pca.P_pca_(3*idx, dcount) * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = weight_model * ( landmarks_pointset[i][1] - pca.m_pca_(3*idx + 1) );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, pca.P_pca_(3*idx + 1, dcount) * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = weight_model * ( landmarks_pointset[i][2] - pca.m_pca_(3*idx + 2) );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, pca.P_pca_(3*idx + 2, dcount) * weight_model) ); // d's
        }
        ++r;
    }
}

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


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

    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > headmodel_landmarks_boundary
        = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:mesh_boundary_landmarks");

    if (!headmodel_landmarks_boundary)
    {
        std::cerr << "Energy_term_pca_boundary::n_rows_big_sle(): [ERROR] !headmodel_landmarks_boundary." << std::endl;
        return 0;
    }

    const std::vector<Surface_mesh::Vertex>& headmodel_landmarks_boundary_vec = headmodel_landmarks_boundary[0];

    return (unsigned int) 3*headmodel_landmarks_boundary_vec.size();
}


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