#include <math.h>

#define PI 3.14159265359

struct Quaternion {
public:
     float w,x,y,z;

public:
     // constructor
     Quaternion();
     Quaternion(float qw, float qx, float qy, float qz);

     // FromAxisAngle - creates a quaternion to do a rotation,
     //     from an <x,y,z> axis and an angle in degrees
     static Quaternion FromAxisAngle(float angle, float ax, float ay, float az); 

     // GetConjugate - returns the conjugate of the current quaternion
     Quaternion GetConjugate();

     // Multiply - Multiplies two quaternions togeather
     Quaternion Multiply(Quaternion q2);

     // RotatePoint - Rotates the given point by the current quaternion
     Quaternion RotatePoint(float x, float y, float z);

     // Slerp - Performs Spherical Linear IntERPolation
     Quaternion Slerp(Quaternion q2, float time);
};

Quaternion::Quaternion(){
}

Quaternion::Quaternion(float qw, float qx, float qy, float qz){
     w = qw; x = qx; y = qy; z = qz;
}

Quaternion Quaternion::FromAxisAngle(float angle, float ax, float ay, float az){
     float sinangle, cosangle;
     float length;

     // we need to normalize the axis values so that they are a unit vector
     length = (float) sqrt(ax*ax+ay*ay+az*az);
     ax /= length;
     ay /= length;
     az /= length;

     // precalculate the sin and cos of the angle so we dont need to keep using the sin function
     // NOTE: sin uses radians, and we want degrees as angles, so the formula is
     //     angle = angle * (PI / 180);
     //     sinangle = sin(angle/2);
     // which is the same as saying
     //     sinangle = sin(angle * PI/360);
     sinangle = (float) sin(angle * PI / 360.0);
     cosangle = (float) cos(angle * PI / 360.0);

     return Quaternion(cosangle, ax * sinangle, ay * sinangle, az * sinangle);
}

Quaternion Quaternion::GetConjugate(){
     return Quaternion(w,-x,-y,-z);
}

Quaternion Quaternion::Multiply(Quaternion q2){
     Quaternion Result;
     float w1, x1, y1, z1;
     float w2, x2, y2, z2;

     w1 = w; x1 = x; y1 = y; z1 = z;
     w2 = q2.w; x2 = q2.x; y2 = q2.y; z2 = q2.z;

     Result.w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
     Result.x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
     Result.y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
     Result.z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;

     return Result;
}

Quaternion Quaternion::RotatePoint(float x, float y, float z){
     Quaternion p(0,x,y,z);
     Quaternion cq = GetConjugate();
     Quaternion result;

     // now for the magic (q * P * q')
     result = Multiply(p).Multiply(cq);
     return result;
}

Quaternion Quaternion::Slerp(Quaternion q2, float time){
     float dotprod;
     float angle;
     double sinangle;
     float a, b;
     Quaternion result;
     float w1, x1, y1, z1;
     float w2, x2, y2, z2;

     w1 = w; x1 = x; y1 = y; z1 = z;
     w2 = q2.w; x2 = q2.x; y2 = q2.y; z2 = q2.z;

     // get the dot product, and ensure it's >= 0
     // to ensure the shortest path (< 180) is chosen
     dotprod = w1*w2 + x1*x2 + y1*y2 + z1*z2;
     if(dotprod < 0.0f){
          w1 = -w1; x1 = -x1; y1 = -y1; z1 = -z1;
          dotprod = w1*w2 + x1*x2 + y1*y2 + z1*z2;
     }

     // get the angle
     angle = (float) acos(dotprod);
     if(angle == 0){
          return Quaternion(w1,x1,y1,z1);
     }
     sinangle = sqrt(1 - (dotprod * dotprod));

     // calculate the multipliers
     a = (float)(sin(angle * (1.0f - time)) / sinangle);
     b = (float)(sin(angle * time) / sinangle);

     // create the result quaternion
     return Quaternion(a*w1+b*w2, a*x1+b*x2, a*y1+b*y2, a*z1+b*z2);
}