//q1 and q2 must always have values within [0,2*PI]
//strong assumption here that q1 and q2 are not displaced by much from each other
double getQuaternionDiffShortestRad(btQuaternion q1, btQuaternion q2)
{
  double th1 = q1.getAngle();
  double th2 = q2.getAngle();

  double diffval = (th1-th2);
  ///double diffval1 = (th1-th2);

  /*
  cout << "th1: " << (th1/PI*180.0) << "\n";
  cout << "th2: " << (th2/PI*180.0) << "\n";
  */

  if((diffval<(-PI))&&((th1<=(PI/2.0))&&(th1>=0.0))&&((th2>=(3*PI/2.0))&&(th2<=(2*PI))))
    {
      diffval = (diffval+2*PI);
    }
  else if((diffval>PI)&&((th2<=(PI/2.0))&&(th2>=0.0))&&((th1>=(3*PI/2.0))&&(th1<=(2*PI))))
    {
      diffval = (diffval-2*PI);
    }

  ///cout << "2.diffval: " << (diffval/PI*180.0) << "\n";

  /*
  if(diffval<(-PI))
    diffval=(diffval+2*PI);
  else if(diffval>PI)
    diffval=(diffval-2*PI);
  */

  ///cout << "diffval: " << (diffval/PI*180.0) << "\n";

  return diffval;
}

//q1 and q2 must always have values within [0,2*PI]
btQuaternion getQuaternionDiff(btQuaternion q1, btQuaternion q2)
{
  double th1 = q1.getAngle();
  double th2 = q2.getAngle();

  double diffval = (th1-th2);

  /*
  if((diffval<(-PI))&&((th1<(PI/2.0))&&(th1>0.0))&&((th2>(3*PI/2.0))&&(th2<(2*PI))))
    {
      diffval = (diffval+2*PI);
    }
  else if((diffval>PI)&&((th2<(PI/2.0))&&(th2>0.0))&&((th1>(3*PI/2.0))&&(th1<(2*PI))))
    {
      diffval = (diffval-2*PI);
    }
  */

  if(diffval<0)
    diffval=(diffval+2*PI);

  btQuaternion diffq;
  diffq.setRPY(0.0,0.0,diffval);

  return diffq;
}

static inline void copy_quat_btquat(float quat[4], const btQuaternion &btquat)
{
	quat[0] = btquat.getW();
	quat[1] = btquat.getX();
	quat[2] = btquat.getY();
	quat[3] = btquat.getZ();
}

void btQuaternion_to_Quaternion(JNIEnv * const &jenv, jobject &target, const btQuaternion & source)
{
	quaternion_ensurefields(jenv, target);
	jenv->SetFloatField(target, quaternion_x, source.getX());
	jenv->SetFloatField(target, quaternion_y, source.getY());
	jenv->SetFloatField(target, quaternion_z, source.getZ());
	jenv->SetFloatField(target, quaternion_w, source.getW());
}

Quaternion::Quaternion(const btQuaternion &aBulletQuaternion)
{
	x = aBulletQuaternion.getX();
	y = aBulletQuaternion.getY();
	z = aBulletQuaternion.getZ();
	w = aBulletQuaternion.getW();

}

// convert quaternion
math::quat convert( const btQuaternion& q )
{
	return math::quat(
		q.x(),
		q.y(),
		q.z(),
		q.w() );
}

// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
												 btScalar& swingAngle, // out
												 btVector3& vSwingAxis, // out
												 btScalar& swingLimit) // out
{
	swingAngle = qCone.getAngle();
	if (swingAngle > SIMD_EPSILON)
	{
		vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
		vSwingAxis.normalize();
#if 0
        // non-zero twist?! this should never happen.
       btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
#endif
        
		// Compute limit for given swing. tricky:
		// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
		// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)

		// For starters, compute the direction from center to surface of ellipse.
		// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
		// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
		btScalar xEllipse =  vSwingAxis.y();
		btScalar yEllipse = -vSwingAxis.z();

		// Now, we use the slope of the vector (using x/yEllipse) and find the length
		// of the line that intersects the ellipse:
		//  x^2   y^2
		//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
		//  a^2   b^2
		// Do the math and it should be clear.

		swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
		if (fabs(xEllipse) > SIMD_EPSILON)
		{
			btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
			btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
			norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
			btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
			swingLimit = sqrt(swingLimit2);
		}

		// test!
		/*swingLimit = m_swingSpan2;
		if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
		{
		btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
		btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
		btScalar phi = asin(sinphi);
		btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
		btScalar alpha = 3.14159f - theta - phi;
		btScalar sinalpha = sin(alpha);
		swingLimit = m_swingSpan1 * sinphi/sinalpha;
		}*/
	}

	// Taken from Irrlicht page
	btVector3 quatToEuler(const btQuaternion & quat)
	{
		btVector3 ret;
		btScalar w = quat.getW(), x = quat.getX(), y = quat.getY(), z = quat.getZ();
		float ws = w*w, xs = x*x, ys = y*y, zs = z*z;
		ret.setX(atan2f(2.0f*(y*z+x*w), -xs-ys+zs+ws));
		ret.setY(asinf(-2.0f*(x*z-y*w)));
		ret.setZ(atan2f(2.0f*(x*y+z*w), xs-ys-zs+ws));
		ret *= irr::core::RADTODEG;
		return ret;
	}

void BulletDynamicsBody::GetTransform( Vector3& position, Quaternion& rotation )
{
  // get updated parameters
  const btTransform& transform( GetBody()->getWorldTransform() );
  const btVector3& origin( transform.getOrigin() );
  const btQuaternion currentRotation( transform.getRotation() );
  const btVector3& axis( currentRotation.getAxis() );
  const btScalar& angle( currentRotation.getAngle() );

  position = Vector3( origin.x(), origin.y(), origin.z() );
  rotation = Quaternion( float(angle), Vector3( axis.x(), axis.y(), axis.z() ) );
}

static inline btScalar qtdot_ref(btQuaternion& q1, btQuaternion& q2)
{
	return q1.x() * q2.x() +
		   q1.y() * q2.y() +
		   q1.z() * q2.z() +
		   q1.w() * q2.w();
}

static bool IsEqual(const btQuaternion &pt0, const btQuaternion &pt1) {
	float delta = fabs(pt0.x() - pt1.x());
	delta += fabs(pt0.y() - pt1.y());
	delta += fabs(pt0.z() - pt1.z());
	delta += fabs(pt0.w() - pt1.w());
	return delta < 1e-8f;
}

// Converts a quaternion to an euler angle
void QuaternionToEuler(const btQuaternion &tQuat, btVector3 &tEuler) {
  btScalar w = tQuat.getW();
  btScalar x = tQuat.getX();
  btScalar y = tQuat.getY();
  btScalar z = tQuat.getZ();
  float wSquared = w * w;
  float xSquared = x * x;
  float ySquared = y * y;
  float zSquared = z * z;

  tEuler.setX(atan2f(2.0f * (y * z + x * w), -xSquared - ySquared + zSquared + wSquared));
  tEuler.setY(asinf(-2.0f * (x * z - y * w)));
  tEuler.setZ(atan2f(2.0f * (x * y + z * w), xSquared - ySquared - zSquared + wSquared));
  tEuler *= SIMD_DEGS_PER_RAD;
}

void Vec3::setHPR(const btQuaternion& q)
{
    float W = q.getW();
    float X = q.getX();
    float Y = q.getY();
    float Z = q.getZ();
    float WSquared = W * W;
    float XSquared = X * X;
    float YSquared = Y * Y;
    float ZSquared = Z * Z;

    setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
    setY(asinf(-2.0f * (X * Z - Y * W)));
    setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
}   // setHPR(btQuaternion)

geometry_msgs::TransformStamped createTransform(btQuaternion q, btVector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2)
{
  geometry_msgs::TransformStamped t;
  t.header.frame_id = frame1;
  t.child_frame_id = frame2;
  t.header.stamp = stamp;
  t.transform.translation.x = v.x();
  t.transform.translation.y = v.y();
  t.transform.translation.z = v.z();
  t.transform.rotation.x = q.x();
  t.transform.rotation.y = q.y();
  t.transform.rotation.z = q.z();
  t.transform.rotation.w = q.w();
  return t;
}

// Converts a quaternion to an euler angle
void CTBulletHelper::QuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler) {
	btScalar W = TQuat.getW();
	btScalar X = TQuat.getX();
	btScalar Y = TQuat.getY();
	btScalar Z = TQuat.getZ();
	float WSquared = W * W;
	float XSquared = X * X;
	float YSquared = Y * Y;
	float ZSquared = Z * Z;
	
	TEuler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
	TEuler.setY(asinf(-2.0f * (X * Z - Y * W)));
	TEuler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
	TEuler *= core::RADTODEG;
}

void Physics::QuaternionToEuler( const btQuaternion &TQuat, btVector3 &TEuler )
{
	float a[3];

	const float w = TQuat.getW();
	const float x = TQuat.getX();
	const float y = TQuat.getY();
	const float z = TQuat.getZ();
	
	QuaternionToEuler( w, x, y, z, a );

	TEuler.setX( a[0] );
	TEuler.setY( a[1] );
	TEuler.setZ( a[2] );
}

// Converts a quaternion to an euler angle
void _Physics::QuaternionToEuler(const btQuaternion &Quat, btVector3 &Euler) {
	btScalar W = Quat.getW();
	btScalar X = Quat.getX();
	btScalar Y = Quat.getY();
	btScalar Z = Quat.getZ();
	float WSquared = W * W;
	float XSquared = X * X;
	float YSquared = Y * Y;
	float ZSquared = Z * Z;

	Euler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
	Euler.setY(asinf(-2.0f * (X * Z - Y * W)));
	Euler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
	Euler *= irr::core::RADTODEG;
}

//works only if the quaternion represents a pure yaw rotation (yaw axis is the z axis)
double getQuaternionAngleDeg(btQuaternion qtarg)
{
  double th = qtarg.getAngle();

  if(fabs(th)>(2*PI))
    {
      cout << "Bad value returned by qtarg.getAngle, th: " << th << "\n";
      return -9999;
    }

  /*
  if((fabs(qtarg.z())>(1e-6))&&(fabs(qtarg.w())>(1e-6)))
    {
      if((qtarg.z()/qtarg.w())<0)
	{
	  //the angle is in [-PI,0]
	  //converting it to [0,2*PI]
	  th=(th+2*PI);
	}
    }
  */

  /*
  if((qtarg.z()<0)||(qtarg.w()<0))
    {
      //the angle is in [-PI,0]
      //converting it to [0,2*PI]
      th=(th+2*PI);
    }
  */

  double dval = (th/PI*180.0);
  return dval;
}

/* PMDBone::update: update internal transform for current position / rotation */
void PMDBone::update()
{
   btQuaternion r;
   const btQuaternion norot(0.0f, 0.0f, 0.0f, 1.0f);

   m_trans.setOrigin(m_pos + m_offset);
   if (m_type == FOLLOW_ROTATE) {
      /* for co-rotate bone, further apply the rotation of child bone scaled by the rotation weight */
      r = m_rot * norot.slerp(m_childBone->m_rot, m_rotateCoef);
      m_trans.setRotation(r);
   } else {
      m_trans.setRotation(m_rot);
   }
   if (m_parentBone)
      m_trans = m_parentBone->m_trans * m_trans;
}

static void M3x3getRot_ref(const btMatrix3x3 &m, btQuaternion &q)
{
	btVector3 m_el[3] = {m[0], m[1], m[2]};

	btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();

	btScalar temp[4];

	if (trace > btScalar(0.0))
	{
		btScalar s = btSqrt(trace + btScalar(1.0));
		temp[3] = (s * btScalar(0.5));
		s = btScalar(0.5) / s;

		temp[0] = ((m_el[2].y() - m_el[1].z()) * s);
		temp[1] = ((m_el[0].z() - m_el[2].x()) * s);
		temp[2] = ((m_el[1].x() - m_el[0].y()) * s);
	}
	else
	{
		int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0);
		int j = (i + 1) % 3;
		int k = (i + 2) % 3;

		btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
		temp[i] = s * btScalar(0.5);
		s = btScalar(0.5) / s;

		temp[3] = (m_el[k][j] - m_el[j][k]) * s;
		temp[j] = (m_el[j][i] + m_el[i][j]) * s;
		temp[k] = (m_el[k][i] + m_el[i][k]) * s;
	}
	q.setValue(temp[0], temp[1], temp[2], temp[3]);
}