const float Utility::findAngleFromAToB(const tf::Vector3 a, const tf::Vector3 b) const {
  std::vector<float> a_vec;
  a_vec.push_back(a.getX());
  a_vec.push_back(a.getY());

  std::vector<float> b_vec;
  b_vec.push_back(b.getX());
  b_vec.push_back(b.getY());

  return findAngleFromAToB(a_vec, b_vec);
}

    float getVel(tf::Vector3 & pose1, tf::Vector3 & pose2, const ros::Duration & dur)
    {
      float vel;
      float delta_x = fabs(pose1.getX() - pose2.getX());
      float delta_y = fabs(pose1.getY() - pose2.getY());
      float dist = sqrt(delta_x*delta_x + delta_y*delta_y);

      vel = dist / dur.toSec();

      return vel;
    }

std::vector<geometry_msgs::Point> MoveBaseDoorAction::getOrientedFootprint(const tf::Vector3 pos, double theta_cost)
{
  double cos_th = cos(theta_cost);
  double sin_th = sin(theta_cost);
  std::vector<geometry_msgs::Point> oriented_footprint;
  for(unsigned int i = 0; i < footprint_.size(); ++i){
    geometry_msgs::Point new_pt;
    new_pt.x = pos.x() + (footprint_[i].x * cos_th - footprint_[i].y * sin_th);
    new_pt.y = pos.y() + (footprint_[i].x * sin_th + footprint_[i].y * cos_th);
    oriented_footprint.push_back(new_pt);
  }
  return oriented_footprint;
}

tf::Vector3 JPCT_Math::rotate(tf::Vector3 vec, tf::Matrix3x3& mat) {
    float xr = vec.getX() * mat[0][0] + vec.getY() * mat[1][0] + vec.getZ() * mat[2][0];
    float yr = vec.getX() * mat[0][1] + vec.getY() * mat[1][1] + vec.getZ() * mat[2][1];
    float zr = vec.getX() * mat[0][2] + vec.getY() * mat[1][2] + vec.getZ() * mat[2][2];
    vec.setX(xr);
    vec.setY(yr);
    vec.setZ(zr);

    return vec;
}              

double getMinimumDistance(tf::Vector3 position, nav_msgs::GridCells grid) {

  double distance_sq=-1;
  int size = grid.cells.size();
  double my_x=position.getX(), my_y=position.getY();
  double stop_radius_sq = stop_radius*stop_radius;
  for (int i=0; i<size; i++) {
    double d_sq = pow(grid.cells[i].x-my_x,2)+pow(grid.cells[i].y-my_y,2);
    if (distance_sq==-1 || d_sq<distance_sq) distance_sq=d_sq;
    if (d_sq<=stop_radius_sq) return 0;
  }

  return sqrt(distance_sq);
}

 double DistanceFromPlane(
     const tf::Vector3& plane_normal,
     const tf::Vector3& plane_point,
     const tf::Vector3& point)
 {
   return plane_normal.normalized().dot(point - plane_point);
 }

void CameraTransformOptimization::calculateMarkerError(CalibrationState state,
		tf::Vector3 markerPoint, float& error) {
	error = 0;

	for (int i = 0; i < this->measurePoints.size(); i++) {
		float currentError = 0;
		MeasurePoint& current = this->measurePoints[i];
		tf::Transform opticalToFixed = current.opticalToFixed(state);
		tf::Vector3 transformedPoint = opticalToFixed
				* current.measuredPosition;
		currentError += pow(markerPoint.x() - transformedPoint.x(), 2);
		currentError += pow(markerPoint.y() - transformedPoint.y(), 2);
		currentError += pow(markerPoint.z() - transformedPoint.z(), 2);
		error += currentError;
	}
}

 double DistanceFromLineSegment(
     const tf::Vector3& line_start,
     const tf::Vector3& line_end,
     const tf::Vector3& point)
 {    
   return point.distance(ProjectToLineSegment(line_start, line_end, point));
 }

bool Hand_filter::jumped(const tf::Vector3& p_current,const tf::Vector3& p_previous, const float threashold) const{
    if(p_current.distance(p_previous) < threashold){
        return false;
    }else{
        return true;
    }
}

void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){

    if(bFirst){

        origin_buffer.push_back(origin);
        orientation_buffer.push_back(orientation);

        if(origin_buffer.size() == origin_buffer.capacity()){
            bFirst = false;
            ROS_INFO("====== jump filter full ======");
        }

    }else{

        origin_tmp      = origin_buffer[origin_buffer.size()-1];
        orientation_tmp = orientation_buffer[orientation_buffer.size()-1];

        if(bDebug){
            std::cout<< "=== jum debug === " << std::endl;
            std::cout<< "p    : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
            std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
            std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;

            std::cout<< "q    : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() <<  "\t" << orientation.w() << std::endl;
            std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
            std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
        }

        /// Position jump
        if(jumped(origin,origin_tmp,origin_threashold)){
            ROS_INFO("position jumped !");
            origin = origin_tmp;
           // exit(0);
        }else{
            origin_buffer.push_back(origin);
        }

        /// Orientation jump
        if(jumped(orientation,orientation_tmp,orientation_threashold)){
            ROS_INFO("orientation jumped !");
            orientation = orientation_tmp;
            //exit(0);
        }else{
            orientation_buffer.push_back(orientation);
        }
    }
}

tf::Matrix3x3 GetTfRotationMatrix(tf::Vector3 xaxis, tf::Vector3 zaxis) {
    tf::Matrix3x3 m;
    tf::Vector3 yaxis = zaxis.cross(xaxis);
    m.setValue(xaxis.getX(), yaxis.getX(), zaxis.getX(),
            xaxis.getY(), yaxis.getY(), zaxis.getY(),
            xaxis.getZ(), yaxis.getZ(), zaxis.getZ());
    return m;
}

bool HapticsPSM::has_normal_changed(tf::Vector3 &v1, tf::Vector3 &v2){
    tfScalar angle;
    angle = v1.angle(v2);
    if(angle > coll_psm.epsilon){
        return true; //The new normal has changed
    }
    else{
        return false; //The new normal has not changed
    }
}

int extractFrame (const pcl::ModelCoefficients& coeffs,
                  const ARPoint& p1, const ARPoint& p2,
                  const ARPoint& p3, const ARPoint& p4,
                  tf::Matrix3x3 &retmat)
{
    // Get plane coeffs and project points onto the plane
    double a=0, b=0, c=0, d=0;
    if(getCoeffs(coeffs, &a, &b, &c, &d) < 0)
        return -1;

    const tf::Vector3 q1 = project(p1, a, b, c, d);
    const tf::Vector3 q2 = project(p2, a, b, c, d);
    const tf::Vector3 q3 = project(p3, a, b, c, d);
    const tf::Vector3 q4 = project(p4, a, b, c, d);

    // Make sure points aren't the same so things are well-defined
    if((q2-q1).length() < 1e-3)
        return -1;

    // (inverse) matrix with the given properties
    const tf::Vector3 v = (q2-q1).normalized();
    const tf::Vector3 n(a, b, c);
    const tf::Vector3 w = -v.cross(n);
    tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);

    // Possibly flip things based on third point
    const tf::Vector3 diff = (q4-q3).normalized();
    //ROS_INFO_STREAM("w = " << w << " and d = " << diff);
    if (w.dot(diff)<0)
    {
        //ROS_INFO_STREAM("Flipping normal based on p3.  Current value: " << m);
        m[1] = -m[1];
        m[2] = -m[2];
        //ROS_INFO_STREAM("New value: " << m);
    }

    // Invert and return
    retmat = m.inverse();
    //cerr << "Frame is " << retmat << endl;
    return 0;
}

GridRayTrace::GridRayTrace(tf::Vector3 start, tf::Vector3 end, const OccupancyGrid& grid_ref)
	: _x_store(), _y_store()
{
	//Transform (x,y) into map frame
	start = grid_ref.toGridFrame(start);
	end = grid_ref.toGridFrame(end);
	
	double x0 = start.getX(), y0 = start.getY();
	double x1 = end.getX(), y1 = end.getY();
	
	//ROS_WARN("start: (%f, %f) -> end: (%f, %f)", x0, y0, x1, y1);
	
	bresenham
		(
			floor(x0), floor(y0), 
			floor(x1), floor(y1), 
			_x_store, _y_store
		);
	
	_cur_idx = 0;
	_max_idx = std::min(_x_store.size(), _y_store.size());
}

bool planning_environment::configureForAttachedBodyMask(planning_models::KinematicState& state,
        planning_environment::CollisionModels* cm,
        tf::TransformListener& tf,
        const std::string& sensor_frame,
        const ros::Time& sensor_time,
        tf::Vector3& sensor_pos)
{
    state.setKinematicStateToDefault();

    cm->bodiesLock();

    const std::map<std::string, std::map<std::string, bodies::BodyVector*> >& link_att_objects = cm->getLinkAttachedObjects();

    if(link_att_objects.empty()) {
        cm->bodiesUnlock();
        return false;
    }

    planning_environment::updateAttachedObjectBodyPoses(cm,
            state,
            tf);

    sensor_pos.setValue(0.0,0.0,0.0);

    // compute the origin of the sensor in the frame of the cloud
    if (!sensor_frame.empty()) {
        std::string err;
        try {
            tf::StampedTransform transf;
            tf.lookupTransform(cm->getWorldFrameId(), sensor_frame, sensor_time, transf);
            sensor_pos = transf.getOrigin();
        } catch(tf::TransformException& ex) {
            ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", sensor_frame.c_str(), cm->getWorldFrameId().c_str(), ex.what());
            sensor_pos.setValue(0, 0, 0);
        }
    }
    cm->bodiesUnlock();
    return true;
}

tf::Matrix3x3 JPCT_Math::rotateAxis(tf::Vector3 axis, tf::Matrix3x3& original, float angle) {
        
    if(angle != lastRot) {
        lastRot = angle;
        lastSin = (float)sin((double)angle);
        lastCos = (float)cos((double)angle);
    }

    float c = lastCos;
    float s = lastSin;
    float t = 1.0F - c;
    //axis = axis.normalize(axis);
    float x = axis.x();
    float y = axis.y();
    float z = axis.z();
    
    tf::Matrix3x3 mat;
    mat.setIdentity();
    float sy = s * y;
    float sx = s * x;
    float sz = s * z;
    float txy = t * x * y;
    float txz = t * x * z;
    float tyz = t * y * z;
    mat[0][0] = t * x * x + c;
    mat[1][0] = txy + sz;
    mat[2][0] = txz - sy;
    mat[0][1] = txy - sz;
    mat[1][1] = t * y * y + c;
    mat[2][1] = tyz + sx;
    mat[0][2] = txz + sy;
    mat[1][2] = tyz - sx;
    mat[2][2] = t * z * z + c;
    orthonormalize(mat);
    original *= mat;
    
    return original; 
}

bool control(PID_control::controlserver::Request  &req,
	         PID_control::controlserver::Response &ret) {
    tf::vector3MsgToTF(req.target_error,sp);
    tf::transformMsgToTF(req.transform,pose);
    tf::vector3MsgToTF(req.velocity,vel);
    if (req.reset) {
        pParam=2,iParam=0,dParam=0,vParam=-2,cumul=0,lastE=0,pAlphaE=0,pBetaE=0,psp2=0,psp1=0,prevYaw=0;
        ROS_INFO("Controller reset");
    }

	e = (pose*sp).z() - pose.getOrigin().z();
	cumul=cumul+e;
    pv=pParam*e;
    thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+vel.z()*vParam;
    lastE=e;
    // Horizontal control: 
    vx = pose*tf::Vector3(1,0,0);
    vy = pose*tf::Vector3(0,1,0);
    alphaE=(vy.z()-pose.getOrigin().z());
    alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE);
    betaE=(vx.z()-pose.getOrigin().z());
    betaCorr=-0.25*betaE-2.1*(betaE-pBetaE);
    pAlphaE=alphaE;
    pBetaE=betaE;
    alphaCorr=alphaCorr+sp.y()*0.005+1*(sp.y()-psp2);
    betaCorr=betaCorr-sp.x()*0.005-1*(sp.x()-psp1);
    psp2=sp.y();
    psp1=sp.x();
    
    pose.getBasis().getRPY(t1, t2, yaw);
    rotCorr=yaw*0.1+2*(yaw-prevYaw);
    prevYaw = yaw;

    // Decide of the motor velocities:
    a=thrust*((double)1-alphaCorr+betaCorr+rotCorr);
    b=thrust*((double)1-alphaCorr-betaCorr-rotCorr);
    c=thrust*((double)1+alphaCorr-betaCorr+rotCorr);
    d=thrust*((double)1+alphaCorr+betaCorr-rotCorr);
    ret.a=a;
    ret.b=b;
    ret.c=c;
    ret.d=d;
}

  /**
  * @brief Creates a sphere RViz marker for publishing
  * @param color_g The color for the marker, in range of 0 to 1, 0 is pure red, 1 is pure green
  * @param sphere_size The radius of the sphere
  * @param pos The position of the marker on the map
  * @return A marker
  */
  visualization_msgs::Marker prepareSphereMarker(double color_g, double sphere_size, tf::Vector3& pos)
  {
    visualization_msgs::Marker marker;

    marker.header.frame_id = "/map";
    marker.header.stamp = ros::Time::now();
    marker.ns = "wifi_heatmap";
    marker.id = marker_id++;
    marker.type = visualization_msgs::Marker::SPHERE;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = pos.x();
    marker.pose.position.y = pos.y();
    marker.pose.position.z = pos.z();
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker
    marker.scale.x = sphere_size;
    marker.scale.y = sphere_size;
    marker.scale.z = sphere_size;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 1.0 - color_g;
    marker.color.g = color_g;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();

    return marker;
  }

int farthestPoint(const tf::Vector3& point, const std::vector<tf::Vector3>& hand_positions)
{
  double dist, max_distant = 0;
  size_t index = 0;
  for (size_t i = 0; i < hand_positions.size(); i++)
  {
    dist = point.distance(hand_positions[i]);
    if (dist > max_distant)
    {
      max_distant = dist;
      index = i;
    }
  }
  return index;
}

void Hand_filter::align_quaternion_axis(tf::Quaternion& q_out, float &angle, const tf::Quaternion &q_in, tf::Vector3 target){
    tf::Matrix3x3 R;  R.setRotation(q_in);
    tf::Vector3 z_axis(R[0][2],R[1][2],R[2][2]);

    z_axis          = z_axis.normalize();

    tf::Vector3 c   = z_axis.cross(target);
    angle           = z_axis.dot(target);

    tf::Quaternion q_r;
    q_r.setX(c.x());
    q_r.setY(c.y());
    q_r.setZ(c.z());
    q_r.setW(sqrt(z_axis.length2() * target.length2()) + angle);

    q_out = q_r*q_in;
}