/* Method to calculate the IK for the required end pose
 *  @returns true if successful
 */
bool arm_kinematics::Kinematics::getPositionIK(
    const geometry_msgs::PoseStamped &pose_stamp,
    const sensor_msgs::JointState &seed, sensor_msgs::JointState *result) {

  geometry_msgs::PoseStamped pose_msg_in = pose_stamp;
  tf::Stamped<tf::Pose> transform;
  tf::Stamped<tf::Pose> transform_root;
  tf::poseStampedMsgToTF(pose_msg_in, transform);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;

  jnt_pos_in.resize(num_joints);
  // Copying the positions of the joints relative to its index in the KDL chain
  for (unsigned int i = 0; i < num_joints; i++) {
    int tmp_index = getJointIndex(seed.name[i]);
    if (tmp_index >= 0) {
      jnt_pos_in(tmp_index) = seed.position[i];
    } else {
      ROS_ERROR("i: %d, No joint index for %s", i, seed.name[i].c_str());
    }
  }

  //Convert F to our root_frame
  try {
    tf_listener.transformPose(root_name, transform, transform_root);
  } catch (...) {
    ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
    return false;
  }

  KDL::Frame F_dest;
  tf::transformTFToKDL(transform_root, F_dest);

  int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);

  if (ik_valid >= 0) {
    result->name = info.joint_names;
    result->position.resize(num_joints);
    for (unsigned int i = 0; i < num_joints; i++) {
      result->position[i] = jnt_pos_out(i);
      ROS_DEBUG("IK Solution: %s %d: %f", result->name[i].c_str(), i,
                jnt_pos_out(i));
    }
    return true;
  } else {
    ROS_DEBUG("An IK solution could not be found");
    return false;
  }
}
bool KDLArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                              const std::vector<double> &ik_seed_state,
                                              const double &timeout,
                                              std::vector<double> &solution,
                                              const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
                                              const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
                                              int &error_code)  
{
  if(!active_)
  {
    ROS_ERROR("kinematics not active");
    error_code = kinematics::INACTIVE;
    return false;
  }
  KDL::Frame pose_desired;
  tf::PoseMsgToKDL(ik_pose, pose_desired);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;
  jnt_pos_in.resize(dimension_);
  for(unsigned int i=0; i < dimension_; i++)
    jnt_pos_in(i) = ik_seed_state[i];
 
  if(!desired_pose_callback.empty())
    desired_pose_callback(ik_pose,ik_seed_state,error_code);
  
  if(error_code < 0)
  {
    ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
    return false;
  }
  for(int i=0; i < max_search_iterations_; i++)
  {
    jnt_pos_in = getRandomConfiguration();
    int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in,pose_desired,jnt_pos_out);                      
    if(ik_valid < 0)                                                                                                       
      continue;
    std::vector<double> solution_local;
    solution_local.resize(dimension_);
    for(unsigned int j=0; j < dimension_; j++)
      solution_local[j] = jnt_pos_out(j);
    solution_callback(ik_pose,solution_local,error_code);
    if(error_code == kinematics::SUCCESS)
    {
      solution = solution_local;
      return true;
    }
  }
  ROS_DEBUG("An IK that satisifes the constraints and is collision free could not be found");   
  if (error_code == 0)
    error_code = kinematics::NO_IK_SOLUTION;
  return false;
}
bool PR2ArmKinematicsPlugin::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
{
  if(!active_)
  {
    ROS_ERROR("kinematics not active");
    error_code.val = error_code.PLANNING_FAILED; 
    return false;
  }
  KDL::Frame pose_desired;
  Eigen::Affine3d tp;
  tf::poseMsgToEigen(ik_pose, tp);
  tf::transformEigenToKDL(tp, pose_desired);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;
  jnt_pos_in.resize(dimension_);
  for(int i=0; i < dimension_; i++)
  {
    jnt_pos_in(i) = ik_seed_state[i];
  }

  int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
                                                     pose_desired,
                                                     jnt_pos_out,
                                                     timeout);
  if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
  {
    error_code.val = error_code.NO_IK_SOLUTION; 
    return false;
  }

  if(ik_valid >= 0)
  {
    solution.resize(dimension_);
    for(int i=0; i < dimension_; i++)
    {
      solution[i] = jnt_pos_out(i);
    }
    error_code.val = error_code.SUCCESS;
    return true;
  }
  else
  {
    ROS_DEBUG("An IK solution could not be found");   
    error_code.val = error_code.NO_IK_SOLUTION; 
    return false;
  }
}
  bool GaffaArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                                const std::vector<double> &ik_seed_state,
                                                const double &timeout,
                                                std::vector<double> &solution,
                                                const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
                                                const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback)  
  {
    if(!active_)
    {
      ROS_ERROR("kinematics not active");
      return false;
    }
    KDL::Frame pose_desired;
    tf::PoseMsgToKDL(ik_pose, pose_desired);

    desiredPoseCallback_ = desired_pose_callback;
    solutionCallback_    = solution_callback;

    //Do the IK
    KDL::JntArray jnt_pos_in;
    KDL::JntArray jnt_pos_out;
    jnt_pos_in.resize(dimension_);
    for(int i=0; i < dimension_; i++)
    {
        jnt_pos_in(i) = ik_seed_state[i];
    }

    motion_planning_msgs::ArmNavigationErrorCodes error_code;
    int ik_valid = gaffa_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
                                                       pose_desired,
                                                       jnt_pos_out,
                                                       timeout,
                                                       error_code,
                                                       boost::bind(&GaffaArmKinematicsPlugin::desiredPoseCallback, this, _1, _2, _3),
                                                       boost::bind(&GaffaArmKinematicsPlugin::jointSolutionCallback, this, _1, _2, _3));
    if(ik_valid == gaffa_arm_kinematics::NO_IK_SOLUTION)
       return false;

    if(ik_valid >= 0)
    {
      solution.resize(dimension_);
      for(int i=0; i < dimension_; i++)
      {
        solution[i] = jnt_pos_out(i);
      }
      return true;
    }
    else
    {
      ROS_DEBUG("An IK solution could not be found");   
      return false;
    }
  }
//this assumes that everything has been checked and is in the correct frame
bool PR2ArmKinematics::getPositionIKHelper(kinematics_msgs::GetPositionIK::Request &request, 
                                           kinematics_msgs::GetPositionIK::Response &response)
{
  KDL::Frame pose_desired;
  tf::poseMsgToKDL(request.ik_request.pose_stamped.pose, pose_desired);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;
  jnt_pos_in.resize(dimension_);
  for(int i=0; i < dimension_; i++)
  {
    int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_);
    if(tmp_index >=0)
    {
      jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
    }
    else
    {
      ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
    }
  }
  
  int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
                                                     pose_desired,
                                                     jnt_pos_out,
                                                     request.timeout.toSec());
  if(ik_valid == pr2_arm_kinematics::TIMED_OUT)
    response.error_code.val = response.error_code.TIMED_OUT;
  if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
    response.error_code.val = response.error_code.NO_IK_SOLUTION;

  response.solution.joint_state.header = request.ik_request.pose_stamped.header;
  
  if(ik_valid >= 0)
  {
    response.solution.joint_state.name = ik_solver_info_.joint_names;
    response.solution.joint_state.position.resize(dimension_);
    for(int i=0; i < dimension_; i++)
    {
      response.solution.joint_state.position[i] = jnt_pos_out(i);
      ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
    }
    response.error_code.val = response.error_code.SUCCESS;
    return true;
  }
  else
  {
    ROS_DEBUG("An IK solution could not be found");   
    return true;
  }
}
bool LegKinematics::getLegIKSolver (	spider_msgs::GetLegIKSolver::Request &request,
										spider_msgs::GetLegIKSolver::Response &response){

	spider_msgs::LegPositionState leg_dest_pos;
	response.target_joints.clear();

	for (int i = 0; i < request.leg_number.size(); i++){
		leg_dest_pos = request.target_position[i];
		KDL::JntArray jnt_pos_in(num_joints);
		KDL::JntArray jnt_pos_out(num_joints);

		//Get initial joints and frame
		for (unsigned int j=0; j < num_joints; j++) {
			jnt_pos_in(j) = request.current_joints[i].joint[j];
		}
		KDL::Frame F_dest (KDL::Vector(leg_dest_pos.x, leg_dest_pos.y, leg_dest_pos.z));

		//IK solver
		int ik_valid = ik_solver_pos[request.leg_number[i]]->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);

		//ROS_ERROR("---: LEG_IK_SOLVER: %i", ik_valid);
		if (ik_valid >= 0) {
			spider_msgs::LegJointsState jnt_buf;
			for (unsigned int j=0; j<num_joints; j++) {
					jnt_buf.joint[j] = jnt_pos_out(j);
				}
			response.target_joints.push_back(jnt_buf);
			response.error_codes = response.IK_FOUND;
			//ROS_INFO("IK Solution for leg%s found", suffixes[request.leg_number[i]].c_str());
		}
		else {
			response.error_codes = response.IK_NOT_FOUND;
			//ROS_ERROR("An IK solution could not be found");
			return true;
		}
	}
	return true;
}
  bool GaffaArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                                const std::vector<double> &ik_seed_state,
                                                const double &timeout,
                                                std::vector<double> &solution)
  {
    if(!active_)
    {
      ROS_ERROR("kinematics not active");
      return false;
    }
    KDL::Frame pose_desired;
    tf::PoseMsgToKDL(ik_pose, pose_desired);

    //Do the IK
    KDL::JntArray jnt_pos_in;
    KDL::JntArray jnt_pos_out;
    jnt_pos_in.resize(dimension_);
    for(int i=0; i < dimension_; i++)
    {
        jnt_pos_in(i) = ik_seed_state[i];
    }

    int ik_valid = gaffa_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
                                                       pose_desired,
                                                       jnt_pos_out,
                                                       timeout);
    if(ik_valid == gaffa_arm_kinematics::NO_IK_SOLUTION)
       return false;

    if(ik_valid >= 0)
    {
      solution.resize(dimension_);
      for(int i=0; i < dimension_; i++)
      {
        solution[i] = jnt_pos_out(i);
      }
      return true;
    }
    else
    {
      ROS_DEBUG("An IK solution could not be found");   
      return false;
    }
  }
bool KDLArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                              const std::vector<double> &ik_seed_state,
                                              const double &timeout,
                                              std::vector<double> &solution,
                                              int &error_code)
{
  if(!active_)
  {
    ROS_ERROR("kinematics not active");
    error_code = kinematics::INACTIVE;
    return false;
  }
  KDL::Frame pose_desired;
  tf::PoseMsgToKDL(ik_pose, pose_desired);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;
  jnt_pos_in.resize(dimension_);
  for(unsigned int i=0; i < dimension_; i++)
  {
    jnt_pos_in(i) = ik_seed_state[i];
  }

  int ik_valid = ik_solver_pos_->CartToJnt(jnt_pos_in,pose_desired,jnt_pos_out);

  if(ik_valid >= 0)
  {
    solution.resize(dimension_);
    for(unsigned int i=0; i < dimension_; i++)
      solution[i] = jnt_pos_out(i);
    error_code = kinematics::SUCCESS;
    return true;
  }
  else
  {
    ROS_DEBUG("An IK solution could not be found");   
    error_code = kinematics::NO_IK_SOLUTION;
    return false;
  }
}
bool KDLKinematicsPlugin::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 std::vector<double> &consistency_limits,
                                           const kinematics::KinematicsQueryOptions &options) const
{
  ros::WallTime n1 = ros::WallTime::now();
  if(!active_)
  {
    ROS_ERROR_NAMED("kdl","kinematics not active");
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }

  if(ik_seed_state.size() != dimension_)
  {
    ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }

  if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
  {
    ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }

  KDL::JntArray jnt_seed_state(dimension_);
  KDL::JntArray jnt_pos_in(dimension_);
  KDL::JntArray jnt_pos_out(dimension_);

  KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
  KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
  KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
  ik_solver_vel.setMimicJoints(mimic_joints_);
  ik_solver_pos.setMimicJoints(mimic_joints_);

  if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
  {
    ROS_ERROR_NAMED("kdl","Could not set redundant joints");
    return false;
  }

  if(options.lock_redundant_joints)
  {
    ik_solver_vel.lockRedundantJoints();
  }

  solution.resize(dimension_);

  KDL::Frame pose_desired;
  tf::poseMsgToKDL(ik_pose, pose_desired);

  ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
                   ik_pose.position.x << " " <<
                   ik_pose.position.y << " " <<
                   ik_pose.position.z << " " <<
                   ik_pose.orientation.x << " " <<
                   ik_pose.orientation.y << " " <<
                   ik_pose.orientation.z << " " <<
                   ik_pose.orientation.w);
  //Do the IK
  for(unsigned int i=0; i < dimension_; i++)
    jnt_seed_state(i) = ik_seed_state[i];
  jnt_pos_in = jnt_seed_state;

  unsigned int counter(0);
  while(1)
  {
    //    ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
    counter++;
    if(timedOut(n1,timeout))
    {
      ROS_DEBUG_NAMED("kdl","IK timed out");
      error_code.val = error_code.TIMED_OUT;
      ik_solver_vel.unlockRedundantJoints();
      return false;
    }
    int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
    ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
    if(!consistency_limits.empty())
    {
      getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
      if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
      {
        ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
        continue;
      }
    }
    else
    {
      getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
      ROS_DEBUG_NAMED("kdl","New random configuration");
      for(unsigned int j=0; j < dimension_; j++)
        ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));

      if(ik_valid < 0 && !options.return_approximate_solution)
      {
        ROS_DEBUG_NAMED("kdl","Could not find IK solution");
        continue;
      }
    }
    ROS_DEBUG_NAMED("kdl","Found IK solution");
    for(unsigned int j=0; j < dimension_; j++)
      solution[j] = jnt_pos_out(j);
    if(!solution_callback.empty())
      solution_callback(ik_pose,solution,error_code);
    else
      error_code.val = error_code.SUCCESS;

    if(error_code.val == error_code.SUCCESS)
    {
      ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
      ik_solver_vel.unlockRedundantJoints();
      return true;
    }
  }
  ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
  error_code.val = error_code.NO_IK_SOLUTION;
  ik_solver_vel.unlockRedundantJoints();
  return false;
}
bool PR2ArmKinematicsPlugin::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
{
  if(!active_)
  {
    ROS_ERROR("kinematics not active");
    error_code.val = error_code.FAILURE;
    return false;
  }
  if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
  {
    ROS_ERROR("Consistency limits should be of size: %d",dimension_);    
    error_code.val = error_code.FAILURE; 
    return false;
  }

  KDL::Frame pose_desired;
  tf::poseMsgToKDL(ik_pose, pose_desired);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;
  jnt_pos_in.resize(dimension_);
  for(int i=0; i < dimension_; i++)
  {
    jnt_pos_in(i) = ik_seed_state[i];
  }

  int ik_valid;
  if(consistency_limits.empty())
  {
    ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
                                                   pose_desired,
                                                   jnt_pos_out,
                                                   timeout,
                                                   error_code,
                                                   solution_callback ? 
                                                   boost::bind(solution_callback, _1, _2, _3):
                                                   IKCallbackFn());      
  }
  else
  {
    ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
                                                   pose_desired,
                                                   jnt_pos_out,
                                                   timeout,
                                                   consistency_limits[free_angle_],
                                                   error_code,
                                                   solution_callback ? 
                                                   boost::bind(solution_callback, _1, _2, _3):
                                                   IKCallbackFn());    
  }

  if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
    return false;

  if(ik_valid >= 0)
  {
    solution.resize(dimension_);
    for(int i=0; i < dimension_; i++)
    {
      solution[i] = jnt_pos_out(i);
    }
    return true;
  }
  else
  {
    ROS_DEBUG("An IK solution could not be found");   
    return false;
  }
}