#include <ur_kinematics/ur_moveit_plugin.h>
#include <geometry_msgs/PoseStamped.h>
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <ur_kinematics/ur_kin.h>

//register URKinematicsPlugin as a KinematicsBase implementation
PLUGINLIB_EXPORT_CLASS(ur_kinematics::URKinematicsPlugin,kinematics::KinematicsBase);

namespace ur_kinematics
{
  URKinematicsPlugin::URKinematicsPlugin() {}
  //URKinematicsPlugin::~URKinematicsPlugin() {}

  bool URKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
                                         const std::vector<double> &ik_seed_state,
                                         std::vector<double> &solution,
                                         moveit_msgs::MoveItErrorCodes &error_code,
                                         const kinematics::KinematicsQueryOptions &options) const
  {}

  bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                            const std::vector<double> &ik_seed_state,
                                            double timeout,
                                            std::vector<double> &solution,
                                            moveit_msgs::MoveItErrorCodes &error_code,
                                            const kinematics::KinematicsQueryOptions &options) const
  {}

  bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                            const std::vector<double> &ik_seed_state,
                                            double timeout,
                                            const std::vector<double> &consistency_limits,
                                            std::vector<double> &solution,
                                            moveit_msgs::MoveItErrorCodes &error_code,
                                            const kinematics::KinematicsQueryOptions &options) const
  {}

  bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                            const std::vector<double> &ik_seed_state,
                                            double timeout,
                                            std::vector<double> &solution,
                                            const IKCallbackFn &solution_callback,
                                            moveit_msgs::MoveItErrorCodes &error_code,
                                            const kinematics::KinematicsQueryOptions &options) const
  {}

  bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                            const std::vector<double> &ik_seed_state,
                                            double timeout,
                                            const std::vector<double> &consistency_limits,
                                            std::vector<double> &solution,
                                            const IKCallbackFn &solution_callback,
                                            moveit_msgs::MoveItErrorCodes &error_code,
                                            const kinematics::KinematicsQueryOptions &options) const
  {}

  bool URKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
                                         const std::vector<double> &joint_angles,
                                         std::vector<geometry_msgs::Pose> &poses) const
  {
    double q[6];
    for(int i=0; i<6; i++)
      q[i] = joint_angles[i];
    double T[6][16];
    forward_all(q, T[0], T[1], T[2], T[3], T[4], T[5]);
    for(int i=0; i<6; i++) {
      aa
    }
  }

  /**
* @brief Initialization function for the kinematics
* @return True if initialization was successful, false otherwise
*/
  bool URKinematicsPlugin::initialize(const std::string& robot_description,
                                      const std::string& group_name,
                                      const std::string& base_frame,
                                      const std::string& tip_frame,
                                      double search_discretization)
  { 
    setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);

    fk_solver_info_.joint_names.push_back("shoulder_pan_joint");
    fk_solver_info_.joint_names.push_back("shoulder_lift_joint");
    fk_solver_info_.joint_names.push_back("elbow_joint");
    fk_solver_info_.joint_names.push_back("wrist_1_joint");
    fk_solver_info_.joint_names.push_back("wrist_2_joint");
    fk_solver_info_.joint_names.push_back("wrist_3_joint");

    fk_solver_info_.link_names.push_back("base_link");
    fk_solver_info_.link_names.push_back("shoulder_link");
    fk_solver_info_.link_names.push_back("upper_arm_link");
    fk_solver_info_.link_names.push_back("forearm_link");
    fk_solver_info_.link_names.push_back("wrist_1_link");
    fk_solver_info_.link_names.push_back("wrist_2_link");
    fk_solver_info_.link_names.push_back("wrist_3_link");
    fk_solver_info_.link_names.push_back("ee_link");

    return true;
  }

  /**
* @brief Return all the joint names in the order they are used internally
*/
  const std::vector<std::string>& URKinematicsPlugin::getJointNames() const
  {
    return fk_solver_info_.joint_names;
  }

  /**
* @brief Return all the link names in the order they are represented internally
*/
  const std::vector<std::string>& URKinematicsPlugin::getLinkNames() const
  {
    return fk_solver_info_.link_names;
  }
}