/*         ______   ___    ___
 *        /\  _  \ /\_ \  /\_ \ 
 *        \ \ \L\ \\//\ \ \//\ \      __     __   _ __   ___ 
 *         \ \  __ \ \ \ \  \ \ \   /'__`\ /'_ `\/\`'__\/ __`\
 *          \ \ \/\ \ \_\ \_ \_\ \_/\  __//\ \L\ \ \ \//\ \L\ \
 *           \ \_\ \_\/\____\/\____\ \____\ \____ \ \_\\ \____/
 *            \/_/\/_/\/____/\/____/\/____/\/___L\ \/_/ \/___/
 *                                           /\____/
 *                                           \_/__/
 *
 *      Quaternion manipulation routines.
 *
 *      By Jason Wilkins.
 *
 *      See readme.txt for copyright information.
 */


#define ALLEGRO_INCLUDE_MATH_H

#include "allegro.h"



#ifndef M_PI
   #define M_PI   3.14159265358979323846
#endif


#define floatcos(x)   ((float)(cos((x) * M_PI / 128.0)))
#define floatsin(x)   ((float)(sin((x) * M_PI / 128.0)))


#define EPSILON (0.001)



QUAT identity_quat = { 1.0, 0.0, 0.0, 0.0 };



/* quat_mul:
 *  Multiplies two quaternions, storing the result in out. The resulting
 *  quaternion will have the same effect as the combination of p and q,
 *  ie. when applied to a point, (point * out) = ((point * p) * q). Any
 *  number of rotations can be concatenated in this way. Note that quaternion
 *  multiplication is not commutative, ie. quat_mul(p, q) != quat_mul(q, p).
 */
void quat_mul(AL_CONST QUAT *p, AL_CONST QUAT *q, QUAT *out)
{
   QUAT temp;

   /* qp = ww' - v.v' + vxv' + wv' + w'v */

   if (p == out) {
      temp = *p;
      p = &temp;
   }
   else if (q == out) {
      temp = *q;
      q = &temp;
   }

   /* w" = ww' - xx' - yy' - zz' */
   out->w = (p->w * q->w) -
	    (p->x * q->x) -
	    (p->y * q->y) -
	    (p->z * q->z);

   /* x" = wx' + xw' + yz' - zy' */
   out->x = (p->w * q->x) +
	    (p->x * q->w) +
	    (p->y * q->z) -
	    (p->z * q->y);

   /* y" = wy' + yw' + zx' - xz' */
   out->y = (p->w * q->y) +
	    (p->y * q->w) +
	    (p->z * q->x) -
	    (p->x * q->z);

   /* z" = wz' + zw' + xy' - yx' */
   out->z = (p->w * q->z) +
	    (p->z * q->w) +
	    (p->x * q->y) -
	    (p->y * q->x);
}



/* get_x_rotate_quat:
 *  Construct X axis rotation quaternions, storing them in q. When applied to
 *  a point, these quaternions will rotate it about the X axis by the
 *  specified angle (given in binary, 256 degrees to a circle format).
 */
void get_x_rotate_quat(QUAT *q, float r)
{
   q->w = floatcos(r / 2);
   q->x = floatsin(r / 2);
   q->y = 0;
   q->z = 0;
}



/* get_y_rotate_quat:
 *  Construct Y axis rotation quaternions, storing them in q. When applied to
 *  a point, these quaternions will rotate it about the Y axis by the
 *  specified angle (given in binary, 256 degrees to a circle format).
 */
void get_y_rotate_quat(QUAT *q, float r)
{
   q->w = floatcos(r / 2);
   q->x = 0;
   q->y = floatsin(r / 2);
   q->z = 0;
}



/* get_z_rotate_quat:
 *  Construct Z axis rotation quaternions, storing them in q. When applied to
 *  a point, these quaternions will rotate it about the Z axis by the
 *  specified angle (given in binary, 256 degrees to a circle format).
 */
void get_z_rotate_quat(QUAT *q, float r)
{
   q->w = floatcos(r / 2);
   q->x = 0;
   q->y = 0;
   q->z = floatsin(r / 2);
}



/* get_rotation_quat:
 *  Constructs a quaternion which will rotate points around all three axis by
 *  the specified amounts (given in binary, 256 degrees to a circle format).
 */
void get_rotation_quat(QUAT *q, float x, float y, float z)
{
   float sx;
   float sy;
   float sz;
   float cx;
   float cy;
   float cz;
   float cycz;
   float sysz;

   sx = floatsin(x / 2);
   sy = floatsin(y / 2);
   sz = floatsin(z / 2);
   cx = floatcos(x / 2);
   cy = floatcos(y / 2);
   cz = floatcos(z / 2);

   sysz = sy * sz;
   cycz = cy * cz;

   q->w = (cx * cycz) + (sx * sysz);
   q->x = (sx * cycz) - (cx * sysz);
   q->y = (cx * sy * cz) + (sx * cy * sz);
   q->z = (cx * cy * sz) - (sx * sy * cz);
}



/* get_vector_rotation_quat:
 *  Constructs a quaternion which will rotate points around the specified
 *  x,y,z vector by the specified angle (given in binary, 256 degrees to a
 *  circle format).
 */
void get_vector_rotation_quat(QUAT *q, float x, float y, float z, float a)
{
   float l;
   float s;

   l = vector_length_f(x, y, z);

   /* NOTE: Passing (x, y, z) = (0, 0, 0) will cause l to equal 0 which will
    *       cause a divide-by-zero exception. Rotating something about the
    *       zero vector undefined.
    */
   ASSERT(l != 0);

   x /= l;
   y /= l;
   z /= l;

   q->w = floatcos(a / 2);
   s    = floatsin(a / 2);
   q->x = s * x;
   q->y = s * y;
   q->z = s * z;
}



/* quat_to_matrix:
 * Constructs a rotation matrix from a quaternion.
 */
void quat_to_matrix(AL_CONST QUAT *q, MATRIX_f *m)
{
   float ww;
   float xx;
   float yy;
   float zz;
   float wx;
   float wy;
   float wz;
   float xy;
   float xz;
   float yz;

  /* This is the layout for converting the values in a quaternion to a
   * matrix.
   *
   *  | ww + xx - yy - zz       2xy + 2wz             2xz - 2wy     |
   *  |     2xy - 2wz       ww - xx + yy - zz         2yz - 2wx     |
   *  |     2xz + 2wy           2yz - 2wx         ww + xx - yy - zz |
   */

   ww = q->w * q->w;
   xx = q->x * q->x;
   yy = q->y * q->y;
   zz = q->z * q->z;
   wx = q->w * q->x * 2;
   wy = q->w * q->y * 2;
   wz = q->w * q->z * 2;
   xy = q->x * q->y * 2;
   xz = q->x * q->z * 2;
   yz = q->y * q->z * 2;

   m->v[0][0] = ww + xx - yy - zz;
   m->v[1][0] = xy - wz;
   m->v[2][0] = xz + wy;

   m->v[0][1] = xy + wz;
   m->v[1][1] = ww - xx + yy - zz;
   m->v[2][1] = yz - wx;

   m->v[0][2] = xz - wy;
   m->v[1][2] = yz + wx;
   m->v[2][2] = ww - xx - yy + zz;

   m->t[0] = 0.0;
   m->t[1] = 0.0;
   m->t[2] = 0.0;
}



/* matrix_to_quat:
 *  Constructs a quaternion from a rotation matrix. Translation is discarded
 *  during the conversion. Use get_align_matrix_f if the matrix is not
 *  orthonormalized, because strange things may happen otherwise.
 */
void matrix_to_quat(AL_CONST MATRIX_f *m, QUAT *q)
{
   float diag;
   float s;
   int   i;
   int   j;
   int   k;
   float out[4];

   static int next[3] = { 1, 2, 0 };

   diag = m->v[0][0] + m->v[1][1] + m->v[2][2];

   if (diag > 0.0f) {
      s    = (float)(sqrt(diag + 1.0));
      q->w = s / 2.0;
      s    = 0.5 / s;
      q->x = (m->v[1][2] - m->v[2][1]) * s;
      q->y = (m->v[2][0] - m->v[0][2]) * s;
      q->z = (m->v[0][1] - m->v[1][0]) * s;
   }
   else {
      i = 0;

      if (m->v[1][1] > m->v[0][0]) {
	 i = 1;
      }

      if (m->v[2][2] > m->v[i][i]) {
	 i = 2;
      }

      j = next[i];
      k = next[j];

      s = m->v[i][i] - (m->v[j][j] + m->v[k][k]);

      /* NOTE: Passing non-orthonormalized matrices can result in odd things
       *       happening, like the calculation of s below trying to find the
       *       square-root of a negative number, which is imaginary.  Some
       *       implementations of sqrt will crash, while others return this
       *       as not-a-number (NaN). NaN could be very subtle because it will
       *       not throw an exception on Intel processors.
       */
      ASSERT(s > 0.0);

      s = (float)(sqrt(s) + 1.0);

      out[i] = s / 2.0;
      s      = 0.5 / s;
      out[j] = (m->v[i][j] + m->v[j][i]) * s;
      out[k] = (m->v[i][k] + m->v[k][i]) * s;
      out[3] = (m->v[j][k] - m->v[k][j]) * s;

      q->x = out[0];
      q->y = out[1];
      q->z = out[2];
      q->w = out[3];
   }
}



/* quat_conjugate:
 *  A quaternion conjugate is analogous to a complex number conjugate, just
 *  negate the imaginary part
 */
static INLINE void quat_conjugate(AL_CONST QUAT *q, QUAT *out)
{
   /* q^* = w - x - y - z */
   out->w =  (q->w);
   out->x = -(q->x);
   out->y = -(q->y);
   out->z = -(q->z);
}



/* quat_normal:
 *  A quaternion normal is the sum of the squares of the components.
 */
static INLINE float quat_normal(AL_CONST QUAT *q)
{
   /* N(q) = ww + xx + yy + zz */
   return ((q->w * q->w) + (q->x * q->x) + (q->y * q->y) + (q->z * q->z));
}



/* quat_inverse:
 *  A quaternion inverse is the conjugate divided by the normal.
 */
static INLINE void quat_inverse(AL_CONST QUAT *q, QUAT *out)
{
   QUAT  con;
   float norm;

   /* q^-1 = q^* / N(q) */

   quat_conjugate(q, &con);
   norm = quat_normal(q);

   /* NOTE: If the normal is 0 then a divide-by-zero exception will occur, we
    *       let this happen because the inverse of a zero quaternion is
    *       undefined
    */
   ASSERT(norm != 0);

   out->w = con.w / norm;
   out->x = con.x / norm;
   out->y = con.y / norm;
   out->z = con.z / norm;
}



/* apply_quat:
 *  Multiplies the point (x, y, z) by the quaternion q, storing the result in
 *  (*xout, *yout, *zout). This is quite a bit slower than apply_matrix_f.
 *  So, only use it if you only need to translate a handful of points.
 *  Otherwise it is much more efficient to call quat_to_matrix then use
 *  apply_matrix_f.
 */
void apply_quat(AL_CONST QUAT *q, float x, float y, float z, float *xout, float *yout, float *zout)
{
   QUAT v;
   QUAT i;
   QUAT t;

   /* v' = q * v * q^-1 */

   v.w = 0;
   v.x = x;
   v.y = y;
   v.z = z;

   /* NOTE: Rotating about a zero quaternion is undefined. This can be shown
    *       by the fact that the inverse of a zero quaternion is undefined
    *       and therefore causes an exception below.
    */
   ASSERT(!((x == 0) && (y == 0) && (z == 0)));

   quat_inverse(q, &i);
   quat_mul(&i, &v, &t);
   quat_mul(&t,  q, &v);

   *xout = v.x;
   *yout = v.y;
   *zout = v.z;
}



/* quat_slerp:
 *  Constructs a quaternion that represents a rotation between 'from' and
 *  'to'. The argument 't' can be anything between 0 and 1 and represents
 *  where between from and to the result will be.  0 returns 'from', 1
 *  returns 'to', and 0.5 will return a rotation exactly in between. The
 *  result is copied to 'out'.
 *
 *  The variable 'how' can be any one of the following:
 *
 *      QUAT_SHORT - This equivalent quat_interpolate, the rotation will
 *                   be less than 180 degrees
 *      QUAT_LONG  - rotation will be greater than 180 degrees
 *      QUAT_CW    - rotation will be clockwise when viewed from above
 *      QUAT_CCW   - rotation will be counterclockwise when viewed
 *                   from above
 *      QUAT_USER  - the quaternions are interpolated exactly as given
 */
void quat_slerp(AL_CONST QUAT *from, AL_CONST QUAT *to, float t, QUAT *out, int how)
{
   QUAT   to2;
   double angle;
   double cos_angle;
   double scale_from;
   double scale_to;
   double sin_angle;

   cos_angle = (from->x * to->x) +
	       (from->y * to->y) +
	       (from->z * to->z) +
	       (from->w * to->w);

   if (((how == QUAT_SHORT) && (cos_angle < 0.0)) ||
       ((how == QUAT_LONG)  && (cos_angle > 0.0)) ||
       ((how == QUAT_CW)    && (from->w > to->w)) ||
       ((how == QUAT_CCW)   && (from->w < to->w))) {
      cos_angle = -cos_angle;
      to2.w     = -to->w;
      to2.x     = -to->x;
      to2.y     = -to->y;
      to2.z     = -to->z;
   }
   else {
      to2.w = to->w;
      to2.x = to->x;
      to2.y = to->y;
      to2.z = to->z;
   }

   if ((1.0 - ABS(cos_angle)) > EPSILON) {
      /* spherical linear interpolation (SLERP) */
      angle = acos(cos_angle);
      sin_angle  = sin(angle);
      scale_from = sin((1.0 - t) * angle) / sin_angle;
      scale_to   = sin(t         * angle) / sin_angle;
   }
   else {
      /* to prevent divide-by-zero, resort to linear interpolation */
      scale_from = 1.0 - t;
      scale_to   = t;
   }

   out->w = (float)((scale_from * from->w) + (scale_to * to2.w));
   out->x = (float)((scale_from * from->x) + (scale_to * to2.x));
   out->y = (float)((scale_from * from->y) + (scale_to * to2.y));
   out->z = (float)((scale_from * from->z) + (scale_to * to2.z));
}



