/**
 * An OpenGL implementation of a quaternion.
 * 
 * @author Daniel Staudigel
 */

public class Quaternion {

  public double x, y, z, w;

  public Quaternion() {
    x = 0;
    y = 0;
    z = 0;
    w = 1;
  }

  public Quaternion(Quaternion q) {
    x = q.x;
    y = q.y;
    z = q.z;
    w = q.w;
  }

  public Quaternion(Quaternion q1, Quaternion q2) {
    x = q1.x;
    y = q1.y;
    z = q1.z;
    w = q1.w;

    mul(q2);
  }

  public Quaternion(Vector start, Vector end) {
    set(start, end);
  }

  public Quaternion(Vector axis, double radians) {
    set(axis, radians);
  }

  public Quaternion(Vector omega) {
    set(omega);
  }

  public void set(Vector axis, double radians) {
    double costheta = Math.cos(radians / 2);
    double sintheta = Math.sin(radians / 2);

    double len = axis.length();
    sintheta /= len;

    x = axis.x * sintheta;
    y = axis.y * sintheta;
    z = axis.z * sintheta;
    w = costheta;

    normalize();
  }

  public void set(Vector omega) {
    double len = omega.length();
    double costheta = Math.cos(len / 2);
    double sintheta = Math.sin(len / 2);

    x = (omega.x / len) * sintheta;
    y = (omega.y / len) * sintheta;
    z = (omega.z / len) * sintheta;
    w = costheta;

    normalize();
  }

  public void set(Quaternion q) {
    x = q.x;
    y = q.y;
    z = q.z;
    w = q.w;
  }

  public void set(Vector start, Vector end) {
    start.normalize();
    end.normalize();

    double tx, ty, tz, temp, dist;

    double cost, len, ss;

    // get dot product of two vectors
    cost = start.x * end.x + start.y * end.y + start.z * end.z;

    // check if parallel
    if (cost > 0.99999999999f) {
      // printf("parallel\n");
      x = y = z = 0.0f;
      z = 1.0f;
      return;
    } 
    else if (cost < -0.999999999f) { // check if opposite
      // printf("opposite\n");
      // check if we can use cross product of from vector with [1, 0, 0]
      tx = 0.0;
      ty = start.x;
      tz = -start.y;

      len = Math.sqrt(ty * ty + tz * tz);

      if (len < 0.000000001) {
        // nope! we need cross product of from vector with [0, 1, 0]
        tx = -start.z;
        ty = 0.0;
        tz = start.x;
      }

      // normalize
      temp = tx * tx + ty * ty + tz * tz;

      dist = (double) (1.0 / Math.sqrt(temp));

      tx *= dist;
      ty *= dist;
      tz *= dist;

      x = tx;
      y = ty;
      z = tz;
      w = 0.0;

      return;
    }

    // printf("normal\n");
    // ... else we can just cross two vectors

    tx = start.y * end.z - start.z * end.y;
    ty = start.z * end.x - start.x * end.z;
    tz = start.x * end.y - start.y * end.x;

    temp = tx * tx + ty * ty + tz * tz;

    dist = (double) (1.0 / Math.sqrt(temp));

    tx *= dist;
    ty *= dist;
    tz *= dist;

    // we have to use half-angle formulae (sin^2 t = ( 1 - cos (2t) ) /2)

    ss = Math.sqrt(0.5f * (1.0f - cost));

    tx *= ss;
    ty *= ss;
    tz *= ss;

    // scale the axis to get the normalized quaternion
    x = tx;
    y = ty;
    z = tz;

    // cos^2 t = ( 1 + cos (2t) ) / 2
    // mV[3] part is cosine of half the rotation angle
    w = (double) Math.sqrt(0.5f * (1.0f + cost));
  }

  public void invert() {
    double norm, invNorm;

    norm = x * x + y * y + z * z + w * w;

    invNorm = (double) (1.0 / norm);

    x = -x * invNorm;
    y = -y * invNorm;
    z = -z * invNorm;
    w = w * invNorm;
  }

  public void negate() {
    x = -x;
    y = -y;
    z = -z;
  }

  /**
   * 	 * Multiply something by this quat and store the result in this quat.
   * 	 * 
   * 	 * @param quat
   	 */

  public void premul(Quaternion quat) {
    double tw = (quat.w * w) - (quat.x * x) - (quat.y * y) - (quat.z * z);
    double tx = (quat.w * x) + (quat.x * w) + (quat.y * z) - (quat.z * y);
    double ty = (quat.w * y) - (quat.x * z) + (quat.y * w) + (quat.z * x);
    double tz = (quat.w * z) + (quat.x * y) - (quat.y * x) + (quat.z * w);

    w = tw;
    x = tx;
    y = ty;
    z = tz;

    normalize();
  }

  public void mul(Quaternion quat) {
    double tw = (w * quat.w) - (x * quat.x) - (y * quat.y) - (z * quat.z);
    double tx = (w * quat.x) + (x * quat.w) + (y * quat.z) - (z * quat.y);
    double ty = (w * quat.y) - (x * quat.z) + (y * quat.w) + (z * quat.x);
    double tz = (w * quat.z) + (x * quat.y) - (y * quat.x) + (z * quat.w);

    w = tw;
    x = tx;
    y = ty;
    z = tz;

    normalize();
  }

  public void normalize() {
    double dist, square;

    square = x * x + y * y + z * z + w * w;

    if (square > 0.0) {
      dist = (double) (1.0 / Math.sqrt(square));
      x = x * dist;
      y = y * dist;
      z = z * dist;
      w = w * dist;
    } 
    else {
      x = 0;
      y = 0;
      z = 0;
      w = 1;
    }

  }

  public Vector mul(Vector v) {
    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

    //System.out.printf("Quat: %.2f %.2f %.2f %.2f",quat.x,quat.y,quat.z,quat.w);

    x2 = x + x;
    y2 = y + y;
    z2 = z + z;
    xx = x * x2;
    xy = x * y2;
    xz = x * z2;
    yy = y * y2;
    yz = y * z2;
    zz = z * z2;
    wx = w * x2;
    wy = w * y2;
    wz = w * z2;

    return new Vector(( 1.0 - (yy + zz))*v.x +         (xy - wz)*v.y +         (xz + wy)*v.z,
		               (xy + wz)*v.x + (1.0 - (xx + zz))*v.y +         (yz - wx)*v.z,
	                       (xz - wy)*v.x +         (yz + wx)*v.y + (1.0 - (xx + yy))*v.z);
   
  }

  public Vector unmul(Vector v) {
    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

    //System.out.printf("Quat: %.2f %.2f %.2f %.2f",quat.x,quat.y,quat.z,quat.w);

    x = -x; y = -y; z = -z;
    x2 = x + x;
    y2 = y + y;
    z2 = z + z;
    xx = x * x2;
    xy = x * y2;
    xz = x * z2;
    yy = y * y2;
    yz = y * z2;
    zz = z * z2;
    wx = w * x2;
    wy = w * y2;
    wz = w * z2;
    x = -x; y = -y; z = -z;
    
    return new Vector(( 1.0 - (yy + zz))*v.x +         (xy - wz)*v.y +         (xz + wy)*v.z,
		               (xy + wz)*v.x + (1.0 - (xx + zz))*v.y +         (yz - wx)*v.z,
	                       (xz - wy)*v.x +         (yz + wx)*v.y + (1.0 - (xx + yy))*v.z);
  }

  public Vector axis() {
    double fRadians = Math.acos(w) * 2;
    double sintheta = Math.sin(fRadians / 2);

    if (sintheta == 0) {
      sintheta = 1;
    }

    Vector v = new Vector(x / sintheta, y / sintheta, z / sintheta);
    v.normalize();

    return v;
  }

  public double angle() {
    normalize();

    double len = x * x + y * y + z * z;

    if (len > 0.00001) {
      return 2.0 * Math.acos(w);
    }

    else {
      return 0;
    }
  }
}