//=============================================================================
#ifndef GRAPHENE_IKPOSE_FIT_H
#define GRAPHENE_IKPOSE_FIT_H
//=============================================================================

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


#include "ik/Inverse_kinematics.h"

#include <graphene/geometry/Point_set.h>
#include <graphene/geometry/Matrix3x3.h>
#include <graphene/character/data_structure/Joint.h>
#include <graphene/character/data_structure/Character.h>
#include <graphene/surface_mesh/algorithms/templatefitting/Energy.h>
#include <graphene/surface_mesh/algorithms/templatefitting/utility/my_helper.h>
#include <graphene/surface_mesh/algorithms/templatefitting/types/Fitting_target.h>

#include <map>


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


namespace graphene {
namespace surface_mesh {

using graphene::character::Character;
using graphene::geometry::Point_set;

//== CLASS DEFINITION =========================================================

class IKPose_fit
{
public:
    static int DEBUG_ITER_COUNT;

    IKPose_fit(character::Character* character,
               std::vector<Fitting_target*>& fitting_targets);

    ~IKPose_fit();

    void reset_convergence_state();

    // in: shape can be in TPose or ScanPose; skeleton must be in the same pose
    // out: shape and skeleton in TPose or ScanPose; unskinned properties don't change
    bool start(size_t target_index = 0);
    bool minimization_step(size_t target_index = 0);

    // in: shape in TPose, skeleton in ScanPose
    // out: shape in TPose with up-to-date unskinned properties; skeleton in ScanPose
    bool start_with_pca(const std::string& filename_pca_mean,
                        const std::string& filename_pca,
                        const std::string& filename_skinning_info_pos,
                        const std::string& filename_joint_positions_chargen,
                        const std::string& filename_skinning_info_wd,
                        const bool shapedependent_skinningweights,
                        const bool jointposcorrection_chargen,
                        const double max_dist_for_pca_shape_fit,
                        const int max_normal_deviation_for_pca_shape_fit,
                        const int max_iter,
                        const double tikhonov_weight);

    bool start_with_pca(const std::string& filename_pca_mean,
                        const std::string& filename_pca,
                        const std::string& filename_pca_mapping,
                        const double max_dist_for_pca_shape_fit,
                        const int max_normal_deviation_for_pca_shape_fit,
                        const int max_iter,
                        const double tikhonov_weight);

    bool update_dof(const std::string& fn_template_ik_dof = "");

    void set_render_callback(std::function<void(const std::string&)> render_callback) { render_callback_ = render_callback; }

    void set_lambda(double lambda)
    {
        for (auto* ik_solver : ik_solvers_)
        {
            ik_solver->set_lambda(lambda);
        }
    }
    void set_use_landmarks(bool enable) { use_landmarks_ = enable; }

    void set_use_correspondences(bool enable)
    {
        use_correspondences_ik_ = enable;
        use_correspondences_pca_ = enable;
    }

    void set_use_correspondences_ik(bool enable) { use_correspondences_ik_ = enable; }
    void set_use_correspondences_pca(bool enable) { use_correspondences_pca_ = enable; }
    void set_max_iterations(int iter) { max_iterations_ = iter; }
    void set_convergence_value(double conv_value) { conv_value_ = conv_value; }
    void set_max_normalangle(double angle) { max_normal_angle_ = angle; }
    void set_max_dist(double dist) { max_dist_ = dist; }
    void set_fixed_correspondences(std::vector<Correspondence>* correspondences);

    bool is_converged() { return ((rel_error_ < conv_value_) || (iter_ > max_iterations_)); }

    void copy_pose_parameters_to_target(size_t src_target_index, size_t dst_target_index);

    std::vector<Correspondence>& ik_correspondences() { return ik_correspondences_; }
    Energy& energy() { return energy_; }

private:
    bool set_character(character::Character* character);

    void setup_energy_fitting_targets();

    bool compute_mvc_of_joints();

    bool init();

    bool minimize(size_t target_index = 0);

    // transforms positions and normals from point set
    bool transform_points(geometry::Point_set& point_set,
                          const Mat4f& M);

    // shape-dependend skinning stuff
    bool load_skinning_info(const std::string& filename_skinning_info_pos,
                            const std::string& filename_joint_positions_chargen,
                            const std::string& filename_skinning_info_wd);
    void update_skinning_weights_wrt_mix(Surface_mesh* current_mesh);
    int update_skinning_weights_wrt_nearest(Surface_mesh* current_mesh);
    int update_skinning_weights_wrt_hardcoded_character_generator_model(Surface_mesh* current_mesh);
    bool update_joint_position_MVCs_from_chargen(Surface_mesh* current_mesh,
                                                 int idx_closest_character = -1);



private:
    character::Character* character_;

    std::vector<Fitting_target*> fitting_targets_;
    std::vector<ik::Inverse_kinematics*> ik_solvers_;

    Energy energy_;

    int    max_iterations_;
    double conv_value_;
    double max_normal_angle_;
    double max_dist_;

    bool reset_to_pca_mean_;
    bool with_pca_;

    bool use_landmarks_;
    bool use_correspondences_ik_;
    bool use_correspondences_pca_;

    std::vector<Correspondence> ik_correspondences_;
    std::vector<Correspondence>* fixed_correspondences_;

    bool is_initialized_;

    // helper for checking convergence
    int          iter_;
    double      error_;
    double error_prev_;
    double  rel_error_;

    bool do_similarity_transform_;

    // shape-dependend skinning stuff
    Eigen::MatrixXd               mat_positions_of_characters_;
    Eigen::VectorXd               mean_character_vec_;
    Eigen::LLT< Eigen::MatrixXd > solver_;
    // skinning info
    // dim 1: n_mesh_vertices
    // dim 2: n_characters
    // dim 3: n_pairs (joint_idx, weight)
    std::vector< std::vector< std::vector< std::pair< uint8_t, float > > > > skinning_info_;

    Eigen::MatrixXd               mat_joint_pos_of_characters_;

    // For visualizing intermediate steps
    std::function<void(const std::string&)> render_callback_;

};

//=============================================================================
} // namespace surface_mesh
} // namespace graphene
//=============================================================================
#endif // GRAPHENE_IKPose_fit_H
//=============================================================================
