/// Spherical Linear Interpolation
/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
/// Reference: Shoemake at SIGGRAPH 89
/// See also
/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
void ofQuaternion::slerp( float t, const ofQuaternion& from, const ofQuaternion& to ) {
	const double epsilon = 0.00001;
	double omega, cosomega, sinomega, scale_from, scale_to ;

	ofQuaternion quatTo(to);
	// this is a dot product

	cosomega = from.asVec4().dot(to.asVec4());

	if ( cosomega < 0.0 ) {
		cosomega = -cosomega;
		quatTo = -to;
	}

	if ( (1.0 - cosomega) > epsilon ) {
		omega = acos(cosomega) ; // 0 <= omega <= Pi (see man acos)
		sinomega = sin(omega) ;  // this sinomega should always be +ve so
		// could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
		scale_from = sin((1.0 - t) * omega) / sinomega ;
		scale_to = sin(t * omega) / sinomega ;
	} else {
		/* --------------------------------------------------
		The ends of the vectors are very close
		we can use simple linear interpolation - no need
		to worry about the "spherical" interpolation
		-------------------------------------------------- */
		scale_from = 1.0 - t ;
		scale_to = t ;
	}

	*this = (from * scale_from) + (quatTo * scale_to);

	// so that we get a Vec4
}

void ofxRotate(ofQuaternion q) {
	//rotation
	float angle;
	ofVec3f axis;
	q.getRotate(angle, axis);
	ofRotate(angle/TWO_PI*360,axis.x,axis.y,axis.z);
}

//------------------------------------------------------------
void Hammer::setRotation(ofQuaternion rotation){
    
    btTransform transform;
    btRigidBody* rigidBody = body.getRigidBody();
    rigidBody->getMotionState()->getWorldTransform( transform );
	
	btQuaternion originRot;
    originRot.setX(rotation.x());
    originRot.setY(rotation.y());
    originRot.setZ(rotation.z());
	originRot.setW(rotation.w());
    
	transform.setRotation(originRot);
	
    rigidBody->getMotionState()->setWorldTransform( transform );
    
}

	void KalmanEuler_<T>::update(const ofQuaternion& q) {
		// warp to appropriate dimension
		ofVec3f euler = q.getEuler();
		for( int i = 0; i < 3; i++ ) {
			float rev = floorf((eulerPrev[i] + 180) / 360.f) * 360;
			euler[i] += rev;
			if( euler[i] < -90 + rev && eulerPrev[i] > 90 + rev ) euler[i] += 360;
			else if( euler[i] > 90 + rev && eulerPrev[i] < -90 + rev ) euler[i] -= 360;
		}
		
		KalmanPosition_<T>::update(euler);
		eulerPrev = euler;
	}

ofxLatLon ofxToLatLon(ofQuaternion q) {
    ofVec3f c;
    ofVec4f v(0,0,-1,0);
    ofMatrix4x4 m;
    q.get(m);
    ofVec4f mv = m*v;
    c.set(mv.x,mv.y,-mv.z);
    c.rotate(90, 0, 0);
    float lat = ofRadToDeg(asin(c.z));
    float lon = ofRadToDeg(-atan2(c.y,c.x))-90;
    if (lon<-180) lon+=360;
    return ofxLatLon(lat,lon);
}

//--------------------------------------------------------------
void ofxBulletCapsule::create( btDiscreteDynamicsWorld* a_world, ofVec3f a_loc, ofQuaternion a_rot, float a_mass, float a_radius, float a_height ) {
	btTransform tr	= ofGetBtTransformFromVec3f( a_loc );
	tr.setRotation( btQuaternion(btVector3(a_rot.x(), a_rot.y(), a_rot.z()), a_rot.w()) );
	
	create( a_world, tr, a_mass, a_radius, a_height );
}

ofQuaternion testApp::lerpQuat(float t, ofQuaternion qa, ofQuaternion qb)
{
	ofQuaternion qm;

	//dot product
	float cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z();
    if (abs(cosHalfTheta) >= 1.0)
    {
        return qa;
    }
    else
    {
        // Calculate temporary values.
        float halfTheta = acos(cosHalfTheta);
        float sinHalfTheta = sqrt(1.0 - cosHalfTheta*cosHalfTheta);
        // if theta = 180 degrees then result is not fully defined
        // we could rotate around any axis normal to qa or qb
        if (fabs(sinHalfTheta) < 0.001){ // fabs is floating point absolute
        	qm.set(
        			(qa.x() * 0.5 + qb.x() * 0.5),
        			(qa.y() * 0.5 + qb.y() * 0.5),
        			(qa.z() * 0.5 + qb.z() * 0.5),
        			(qa.w() * 0.5 + qb.w() * 0.5)
        		);

            return qm;
        }

        float ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
        float ratioB = sin(t * halfTheta) / sinHalfTheta;

        //calculate Quaternion
    	qm.set(
    	        (qa.x() * ratioA + qb.x() * ratioB),
    	        (qa.y() * ratioA + qb.y() * ratioB),
    	        (qa.z() * ratioA + qb.z() * ratioB),
    	        (qa.w() * ratioA + qb.w() * ratioB)
    		);

        return qm;
    }
}

ofVec3f ofxToCartesian(ofQuaternion q) {
    float angle;
    ofVec3f vec;
    q.getRotate(angle, vec);
    return ofVec3f(0,0,1).rotated(angle, vec);
}

string ofxToString(ofQuaternion q) {
    return ofToString(q.x()) + "," + ofToString(q.y()) + "," + ofToString(q.z()) + "," + ofToString(q.w());
}

string DebugUtil::toString(ofQuaternion quat)
{
    
    ofVec3f vec = quat.getEuler();
    return "alpha:" + ofToString(vec.x) + ", beta:" + ofToString(vec.y) + ", gamma:" + ofToString(vec.z);
}

//--------------------------------------------------------------------------
void game::getMatrix( GLfloat * m, ofQuaternion quat ) {
	float x2 = quat.x() * quat.x();
	float y2 = quat.y() * quat.y();
	float z2 = quat.z() * quat.z();
	float xy = quat.x() * quat.y();
	float xz = quat.x() * quat.z();
	float yz = quat.y() * quat.z();
	float wx = quat.w() * quat.x();
	float wy = quat.w() * quat.y();
	float wz = quat.w() * quat.z();
	m[0] = 1.0f - 2.0f * (y2 + z2);
	m[1] = 2.0f * (xy - wz);
	m[2] = 2.0f * (xz + wy);
	m[3] = 0.0f;
	m[4] = 2.0f * (xy + wz);
	m[5] = 1.0f - 2.0f * (x2 + z2);
	m[6] = 2.0f * (yz - wx);
	m[7] = 0.0f;
	m[8] = 2.0f * (xz - wy);
	m[9] = 2.0f * (yz + wx);
	m[10] = 1.0f - 2.0f * (x2 + y2);
	m[11] = 0.0f;
	m[12] = 0.0f;
	m[13] = 0.0f;
	m[14] = 0.0f;
	m[15] = 1.0f;
}