void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) {
  const double q0 = q.w();
  const double q1 = q.x();
  const double q2 = q.y();
  const double q3 = q.z();
  roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
  pitch = asin(2*(q0*q2-q3*q1));
  yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}

void UpperBodyPlanner::msgPose2Eigen(const geometry_msgs::Pose& input, Eigen::Vector3d& translation, Eigen::Quaterniond& q) {
    translation(0) = input.position.x;
    translation(1) = input.position.y;
    translation(2) = input.position.z;
    q.w() = input.orientation.w;
    q.x() = input.orientation.x;
    q.y() = input.orientation.y;
    q.z() = input.orientation.z;
}

Eigen::Vector3d QuaternionToExponentialMap(const Eigen::Quaterniond& quaternion)
{
	Eigen::Vector3d vec = quaternion.vec();
    if (vec.norm() < ITOMP_EPS)
		return Eigen::Vector3d::Zero();

	double theta = 2.0 * std::acos(quaternion.w());
	vec.normalize();
	return theta * vec;
}

Eigen::Isometry3d KDLToEigen(KDL::Frame tf)
{
  Eigen::Isometry3d tf_out;
  tf_out.setIdentity();
  tf_out.translation()  << tf.p[0], tf.p[1], tf.p[2];
  Eigen::Quaterniond q;
  tf.M.GetQuaternion(q.x(), q.y(), q.z(), q.w());
  tf_out.rotate(q);
  return tf_out;
}

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
	mPoseMap.lock();
    vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 
    					     OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;


    Eigen::Quaterniond globalQ;
    globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
    Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
    vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
                              globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
    globalPoseMap[t] = globalPose;
    lastP = globalP;
    lastQ = globalQ;

    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time(t);
    pose_stamped.header.frame_id = "world";
    pose_stamped.pose.position.x = lastP.x();
    pose_stamped.pose.position.y = lastP.y();
    pose_stamped.pose.position.z = lastP.z();
    pose_stamped.pose.orientation.x = lastQ.x();
    pose_stamped.pose.orientation.y = lastQ.y();
    pose_stamped.pose.orientation.z = lastQ.z();
    pose_stamped.pose.orientation.w = lastQ.w();
    global_path.header = pose_stamped.header;
    global_path.poses.push_back(pose_stamped);

    mPoseMap.unlock();
}

/*!
* \brief affine3d2UrdfPose converts an  Eigen affine 4x4 matrix  o represent the pose into a urdf pose 
* vparam pose   eigen Affine3d pose 
* \return   urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const  Eigen::Affine3d &pose) {
    RCS::Pose p;
    p.getOrigin().setX(pose.translation().x());
    p.getOrigin().setY(pose.translation().y());
    p.getOrigin().setZ(pose.translation().z());

    Eigen::Quaterniond q (pose.rotation());
    tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
    //std::cout <<  "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;

    p.setRotation(qtf);
    //std::cout <<  "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;

#if 0
    MatrixEXd m = pose.rotation();
    Eigen::Quaterniond q = EMatrix2Quaterion(m);

    Eigen::Quaterniond q(pose.rotation());
    p.getRotation().setX(q.x());
    p.getRotation().setY(q.y());
    p.getRotation().setZ(q.z());
    p.getRotation().setW(q.w());
    #endif
   return p;
}

void updateVizMarker(int id, Eigen::Vector3d _pose, Eigen::Quaterniond _attitude)
{
	marker[id].pose.position.x = _pose(0);
	marker[id].pose.position.y = _pose(1);
	marker[id].pose.position.z = _pose(2);
	marker[id].pose.orientation.x = _attitude.x();
	marker[id].pose.orientation.y = _attitude.y();
	marker[id].pose.orientation.z = _attitude.z();
	marker[id].pose.orientation.w = _attitude.w();
	pub_body[id].publish( marker[id] );
}

/**
 * Recursion method to be used with traverseTreeTopDown() and recursion parameters
 * of type *Vector3RecursionParams*.
 *
 * Re-arranges the joint-transform of the recursion link's *parent joint*, along with
 * the link's visual/collision/intertial rotations, such that all joints rotate around the axis
 * given in the recursion parameters vector.
 */
int allRotationsToAxisCB(urdf_traverser::RecursionParamsPtr& p)
{
    urdf_traverser::LinkPtr link = p->getLink();
    if (!link)
    {
        ROS_ERROR("allRotationsToAxis: NULL link passed");
        return -1;
    }

    Vector3RecursionParams::Ptr param = baselib_binding_ns::dynamic_pointer_cast<Vector3RecursionParams>(p);
    if (!param)
    {
        ROS_ERROR("Wrong recursion parameter type");
        return -1;
    }

    urdf_traverser::JointPtr joint = link->parent_joint;
    if (!joint)
    {
        ROS_INFO_STREAM("allRotationsToAxis: Joint for link " << link->name << " is NULL, so this must be the root joint");
        return 1;
    }

    Eigen::Vector3d axis = param->vec;

    Eigen::Quaterniond alignAxis;
    if (urdf_traverser::jointTransformForAxis(joint, axis, alignAxis))
    {
        Eigen::Vector3d rotAxis(joint->axis.x, joint->axis.y, joint->axis.z);
        // ROS_INFO_STREAM("Transforming axis "<<rotAxis<<" for joint "<<joint->name<<" with transform "<<urdf_traverser::EigenTransform(alignAxis));

        urdf_traverser::applyTransform(joint, urdf_traverser::EigenTransform(alignAxis), false);

        // the link has to receive the inverse transform, so it stays at the original position
        Eigen::Quaterniond alignAxisInv = alignAxis.inverse();
        urdf_traverser::applyTransform(link, urdf_traverser::EigenTransform(alignAxisInv), true);

        // we also have to fix the child joint's (1st order child joints) transform
        // to correct for this transformation.
        for (std::vector<urdf_traverser::JointPtr>::iterator pj = link->child_joints.begin();
                pj != link->child_joints.end(); pj++)
        {
            urdf_traverser::applyTransform(*pj, urdf_traverser::EigenTransform(alignAxisInv), true);
        }

        // finally, set the rotation axis to the target
        joint->axis.x = axis.x();
        joint->axis.y = axis.y();
        joint->axis.z = axis.z();
    }

    // all good, indicate that recursion can continue
    return 1;
}

double quaternion_get_yaw(const Eigen::Quaterniond &q)
{
	// to match equation from:
	// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
	const double &q0 = q.w();
	const double &q1 = q.x();
	const double &q2 = q.y();
	const double &q3 = q.z();

	return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
}

double igl::angular_distance(
  const Eigen::Quaterniond & A,
  const Eigen::Quaterniond & B)
{
  using namespace igl;
  assert(fabs(A.norm()-1)<FLOAT_EPS && "A should be unit norm");
  assert(fabs(B.norm()-1)<FLOAT_EPS && "B should be unit norm");
  //// acos is always in [0,2*pi)
  //return acos(fabs(A.dot(B)));
  return fmod(2.*acos(A.dot(B)),2.*PI);
}

Eigen::Quaterniond UpperBodyPlanner::Rmat2Quaternion(const Eigen::Matrix3d& inMat_) {
    Eigen::Quaterniond outQ;
    KDL::Rotation convert;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            convert.data[3 * i + j] = inMat_(i,j);
        }
    }
    convert.GetQuaternion(outQ.x(), outQ.y(), outQ.z(), outQ.w());
    return outQ;
}

void planning_models::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
  if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
  {
    Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
    Eigen::Quaterniond q;
    quatFromMsg(transform.transform.rotation, q);
    setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
  } else {
    ROS_ERROR("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
  }
}

void create_data(Eigen::MatrixXd &pa, Eigen::MatrixXd &pb) {
	int n_data = 7;
   	Eigen::MatrixXd pa0(3, n_data);	
   	Eigen::MatrixXd pb0(3, n_data);	
	
	pb0 << -0.7045189014502934,0.31652495664145264,-0.8913587885243552,0.4196143278053829,0.33125081405575785,-1.148712511573519,-0.7211957446166447,-0.4204243223315903,-0.8922857301575797,0.41556308950696674,-0.36760757371251074,-1.1630155401570457,-0.12535642300333297,0.26708755761917147,1.5095344824450356,0.9968448409012386,0.27593113974268946,1.2189108175890786,-0.28095118914331707,-0.40276257201497045,1.3669272703783852; 
	
	Eigen::Quaterniond q = Eigen::Quaterniond(2.86073, 0.0378363, 3.59752, 0.4211619).normalized();
	std::cout << "groundtruth-quaternion. w: " << q.w() << " x " << q.x() << " y: " << q.y() << " z " << q.z() << std::endl;
	pa = q.toRotationMatrix()*pb0;
	pb = pb0;
}

void robot_state::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
  if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
  {
    Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
    Eigen::Quaterniond q;
    tf::quaternionMsgToEigen(transform.transform.rotation, q);
    setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
  } else {
    logError("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
  }
}

void LCM2ROS::neckPitchHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const drc::neck_pitch_t* msg)
{
  ROS_ERROR("LCM2ROS got desired neck pitch");
  ihmc_msgs::HeadOrientationPacketMessage mout;
  Eigen::Quaterniond quat = euler_to_quat(0, msg->pitch, 0);
  mout.trajectory_time = 1;
  mout.orientation.w = quat.w();
  mout.orientation.x = quat.x();
  mout.orientation.y = quat.y();
  mout.orientation.z = quat.z();
  mout.unique_id = msg->utime;
  neck_orientation_pub_.publish(mout);
}

Eigen::Matrix3d FeatureModel::dR_by_dqz(const Eigen::Quaterniond &q)
{
  Eigen::Matrix3d M;

  M << -2*q.z(), -2*q.w(), 2*q.x(),
        2*q.w(), -2*q.z(), 2*q.y(),
        2*q.x(), 2*q.y(), 2*q.z();

  return  M;
}

inline void ICP::UpdateXopt(Eigen::VectorXd &xopt, const Eigen::Matrix4d& Tr){
	
	Eigen::Quaterniond q (Tr.block<3,3>(0,0));
 	
	xopt(0) = q.x();		//rx
	xopt(1) = q.y();		//ry
	xopt(2) = q.z();		//rz
	xopt(3) = q.w();		//teta
   	
	xopt(4) = Tr(0,3);	//tx
	xopt(5) = Tr(1,3);	//ty		
	xopt(6) = Tr(2,3);	//tz
	
}

void VideoIMUFusionDevice::handleIMUVelocity(
    const OSVR_TimeValue &timestamp, const OSVR_AngularVelocityReport &report) {

    using namespace osvr::util::eigen_interop;
    Eigen::Quaterniond q = map(report.state.incrementalRotation);
    Eigen::Vector3d rot;
    if (q.w() >= 1. || q.vec().isZero(1e-10)) {
        rot = Eigen::Vector3d::Zero();
    } else {
#if 0
        auto magnitude = q.vec().blueNorm();
        rot = (q.vec() / magnitude * (2. * std::atan2(magnitude, q.w()))) /
              report.state.dt;
#else
        auto angle = std::acos(q.w());
        rot = q.vec().normalized() * angle * 2 / report.state.dt;
#endif
    }

    m_fusion.handleIMUVelocity(timestamp, rot);
    if (m_fusion.running()) {
        sendMainPoseReport();
    }
    sendVelocityReport();
}

int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose,
                                   double gripper_opening,
                                   double gripper_pitch,
                                   double x_offset,
                                   double z_offset,
                                   double quality)
{
  moveit_msgs::Grasp grasp;
  grasp.grasp_pose = pose;

  // defaults
  grasp.pre_grasp_posture = makeGraspPosture(gripper_opening);
  grasp.grasp_posture = makeGraspPosture(0.0);
  grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_,
                                                    approach_min_translation_,
                                                    approach_desired_translation_);
  grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_,
                                                    retreat_min_translation_,
                                                    retreat_desired_translation_,
                                                    -1.0);  // retreat is in negative x direction

  // initial pose
  Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x,
                                           pose.pose.position.y,
                                           pose.pose.position.z) *
                        Eigen::Quaterniond(pose.pose.orientation.w,
                                           pose.pose.orientation.x,
                                           pose.pose.orientation.y,
                                           pose.pose.orientation.z);
  // translate by x_offset, 0, z_offset
  p = p * Eigen::Translation3d(x_offset, 0, z_offset);
  // rotate by 0, pitch, 0
  p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0);
  // apply grasp point -> planning frame offset
  p = p * Eigen::Translation3d(-tool_offset_, 0, 0);

  grasp.grasp_pose.pose.position.x = p.translation().x();
  grasp.grasp_pose.pose.position.y = p.translation().y();
  grasp.grasp_pose.pose.position.z = p.translation().z();
  Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear();
  grasp.grasp_pose.pose.orientation.x = q.x();
  grasp.grasp_pose.pose.orientation.y = q.y();
  grasp.grasp_pose.pose.orientation.z = q.z();
  grasp.grasp_pose.pose.orientation.w = q.w();

  grasp.grasp_quality = quality;

  grasps_.push_back(grasp);
  return 1;
}

static void subject_publish_callback(const ViconDriver::Subject &subject)
{
  if(!running)
    return;

  static std::map<std::string, ros::Publisher> vicon_publishers;
  std::map<std::string, ros::Publisher>::iterator it;

  it = vicon_publishers.find(subject.name);
  if(it == vicon_publishers.end())
  {
    ros::Publisher pub = nh->advertise<vicon::Subject>(subject.name, 10);
    it = vicon_publishers.insert(std::make_pair(subject.name, pub)).first;
  }

  if(loadCalib(subject.name))
  {
    Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
    current_pose.translate(Eigen::Vector3d(subject.translation));
    current_pose.rotate(Eigen::Quaterniond(subject.rotation));
    current_pose = current_pose * calib_pose[subject.name];
    const Eigen::Vector3d position(current_pose.translation());
    const Eigen::Quaterniond rotation(current_pose.rotation());

    vicon::Subject subject_ros;
    subject_ros.header.seq = subject.frame_number;
    subject_ros.header.stamp = ros::Time(subject.time_usec / 1e6);
    subject_ros.header.frame_id = "/vicon";
    subject_ros.name = subject.name;
    subject_ros.occluded = subject.occluded;
    subject_ros.position.x = position.x();
    subject_ros.position.y = position.y();
    subject_ros.position.z = position.z();
    subject_ros.orientation.x = rotation.x();
    subject_ros.orientation.y = rotation.y();
    subject_ros.orientation.z = rotation.z();
    subject_ros.orientation.w = rotation.w();
    subject_ros.markers.resize(subject.markers.size());
    for(size_t i = 0; i < subject_ros.markers.size(); i++)
    {
      subject_ros.markers[i].name = subject.markers[i].name;
      subject_ros.markers[i].subject_name = subject.markers[i].subject_name;
      subject_ros.markers[i].position.x = subject.markers[i].translation[0];
      subject_ros.markers[i].position.y = subject.markers[i].translation[1];
      subject_ros.markers[i].position.z = subject.markers[i].translation[2];
      subject_ros.markers[i].occluded = subject.markers[i].occluded;
    }
    it->second.publish(subject_ros);
  }
}