#ifndef INVERSE_KINEMATICS_H
#define INVERSE_KINEMATICS_H

#include <Eigen/Geometry>
#include <Eigen/Dense>

#include <graphene/geometry/Matrix3x3.h>
#include <graphene/character/data_structure/Joint.h>
#include <graphene/character/data_structure/Skeleton.h>

#include <map>
#include <vector>


namespace ik {

class Inverse_kinematics
{
private:

    struct Effector
    {
        Effector(graphene::character::Joint* joint,
                 const graphene::Vec3f& position,
                 const graphene::Vec3f& target,
                 double weight = 1.0)
            : joint_(joint), position_(position), target_(target), weight_(weight)
        {
            // compute offset
            graphene::Vec3f dir = position - joint_->get_global_translation();
            offset_ = inverse(graphene::Mat3f(joint_->global_)) * dir;
        }

        graphene::character::Joint* joint_;
        graphene::Vec3f position_;
        graphene::Vec3f target_;
        graphene::Vec3f offset_;
        double          weight_;
    };

    struct DoF
    {
        DoF()
            : index_(-1), joint_(NULL), axis_(graphene::Vec3f(0)),
              min_(-2*M_PI),
              max_(2*M_PI),
              has_limits_(false)
        {}

        DoF(int idx,
            graphene::character::Joint* j,
            const graphene::Vec3f& a)
            : index_(idx), joint_(j), axis_(a),
              min_(-2*M_PI),
              max_(2*M_PI),
              has_limits_(false)
        {}

        DoF(int idx,
            graphene::character::Joint* j,
            const graphene::Vec3f& a,
            double min,
            double max)
            : index_(idx), joint_(j), axis_(a),
              min_(min), max_(max),
              has_limits_(true)
        {}

        /// the index of this DoF (column index for Jacobian matrix)
        int index_;

        /// the joint
        graphene::character::Joint* joint_;

        /// the rotation axis (x, y, or z) of this DoF
        graphene::Vec3f axis_;

        double min_;
        double max_;

        bool has_limits_;
    };

    DoF invalid_dof;


public:

    Inverse_kinematics(double lambda = 1.0);

    ~Inverse_kinematics();

    bool set_targets_and_positions(const std::vector<graphene::Vec3f>& targets,
                                   const std::vector<graphene::Vec3f>& positions,
                                   const std::vector<double>& weights = std::vector<double>());

    bool solve_IK();

    void set_clamping(bool clamp) { clamp_ = clamp; }

    void set_lambda(double lambda) { lambda_ = lambda; }

    void clear_effectors();

    const std::vector<double>& get_current_parameters();

    bool set_parameters(const std::vector<double>& parameters);

    std::vector<double> constrain(const std::vector<double>& param) const;

    bool load_dof(const std::string& filename);

    bool collect_dofs();

    void set_skeleton(graphene::character::Skeleton* skeleton);


private:

    const ik::Inverse_kinematics::DoF& get_joint_info(int index) const;

    void add_effector(graphene::character::Joint* joint,
                      const graphene::Vec3f& position,
                      const graphene::Vec3f& target,
                      const double weight = 1.0);

    bool has_effectors(const std::string &joint);

    void reset();

    void transform_joints(const std::vector<double>& parameters);

    bool setup();
    Eigen::MatrixXd damping_matrix();
    bool compute_update();
    bool apply_update();

    void move_effectors();

    bool move(const std::vector<double>& parameters);


private:

    graphene::character::Skeleton* skeleton_;

    std::vector<ik::Inverse_kinematics::Effector *> effectors_;
    // mapping from joint name to vector of effectors
    std::map<std::string, std::vector<ik::Inverse_kinematics::Effector *> > map_effectors_;

    double lambda_;

    Eigen::VectorXd errors_;
    Eigen::MatrixXd jacobian_;

    // keep track of current pose parameters for joint limit avoidance
    std::vector<double> current_;

    /// update to joint angles
    Eigen::VectorXd dtheta_;

    bool clamp_;

    std::vector<double> hprev_;

    // mapping from joint name to vector of DoFs
    std::map< std::string, std::vector<ik::Inverse_kinematics::DoF> > dofs_;

    // free joints as specified by filename when load_dof(filename) is called
    std::vector<std::string>                                            free_joints_;
    // joint limits as specified by filename when load_dof(filename) is called
    // joint_limits_ may be larger than free_joints since the free joints
    // may or may not have joint limits
    std::map<std::string, std::pair<graphene::Vec3f, graphene::Vec3f> > joint_limits_;


public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};

} // namespace ik

#endif
