/*
Copyright (C) 2001, 2006 United States Government
as represented by the Administrator of the
National Aeronautics and Space Administration.
All Rights Reserved.
*/
package gov.nasa.worldwind.geom;

/**
 * @author Chris Maxwell
 * @version $Id: Quaternion.java 1749 2007-05-06 19:48:14Z tgaskins $
 */
public class Quaternion
{
    private final double x;
    private final double y;
    private final double z;
    private final double w;
    private final static double epsilon = 0.0001;

    public Quaternion(double x, double y, double z, double w)
    {
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }

    @Override
    public boolean equals(Object o)
    {
        if (this == o)
            return true;
        if (o == null || getClass() != o.getClass())
            return false;

        final gov.nasa.worldwind.geom.Quaternion quaternion = (gov.nasa.worldwind.geom.Quaternion) o;

        if (Double.compare(quaternion.w, w) != 0)
            return false;
        if (Double.compare(quaternion.x, x) != 0)
            return false;
        if (Double.compare(quaternion.y, y) != 0)
            return false;
        if (Double.compare(quaternion.z, z) != 0)
            return false;

        return true;
    }

    /**
     * Generates an integer that is always the same for identical objects, but usually different for different objects.
     * This method overrides the one in <code>Object</code>.
     * <p/>
     * This method makes comparisons on private fields; overriding implementations should include a call to
     * <code>super.hashCode()</code>.
     *
     * @return the hashCode for this <code>Point</code>.
     */
    @Override
    public int hashCode()
    {
        int result;
        long temp;
        temp = x != +0.0d ? Double.doubleToLongBits(x) : 0L;
        result = (int) (temp ^ (temp >>> 32));
        temp = y != +0.0d ? Double.doubleToLongBits(y) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = z != +0.0d ? Double.doubleToLongBits(z) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        temp = w != +0.0d ? Double.doubleToLongBits(w) : 0L;
        result = 29 * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public final String toString()
    {
        return "(" + Double.toString(this.x) + ", " + Double.toString(this.y) + ", " + Double.toString(
            this.z) + ", " + Double.toString(this.w) + ")";
    }

    public static Quaternion EulerToQuaternion(double yaw, double pitch, double roll)
    {
        double cy = Math.cos(yaw * 0.5);
        double cp = Math.cos(pitch * 0.5);
        double cr = Math.cos(roll * 0.5);
        double sy = Math.sin(yaw * 0.5);
        double sp = Math.sin(pitch * 0.5);
        double sr = Math.sin(roll * 0.5);

        double qw = cy * cp * cr + sy * sp * sr;
        double qx = sy * cp * cr - cy * sp * sr;
        double qy = cy * sp * cr + sy * cp * sr;
        double qz = cy * cp * sr - sy * sp * cr;

        return new Quaternion(qx, qy, qz, qw);
    }

    /**
     * Transforms a rotation in quaternion form to a set of Euler angles
     *
     * @param q
     * @return The rotation transformed to Euler angles, X=Yaw, Y=Pitch, Z=Roll (radians)
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static Point QuaternionToEuler(Quaternion q)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }

        double q0 = q.w;
        double q1 = q.x;
        double q2 = q.y;
        double q3 = q.z;

        double x = Math.atan2(2 * (q2 * q3 + q0 * q1), (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
        double y = Math.asin(-2 * (q1 * q3 - q0 * q2));
        double z = Math.atan2(2 * (q1 * q2 + q0 * q3), (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));

        return new Point(x, y, z);
    }

    public static Quaternion AxisAngleToQuaternion(Angle angle, double x, double y, double z)
    {
        if (angle == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.AngleIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        double length = Math.sqrt(x * x + y * y + z * z);
        if (length > 0 && length != 1)
        {
            x = x / length;
            y = y / length;
            z = z / length;
        }
        double sinAngle = angle.sinHalfAngle();
        double cosAngle = angle.cosHalfAngle();
        return new Quaternion(x * sinAngle, y * sinAngle, z * sinAngle, cosAngle);        
    }

    public static Point QuaternionToAxisAngle(Quaternion q)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        double x, y, z;
        q = q.Normalize();
        double s = Math.sqrt(q.x * q.x + q.y * q.y + q.x * q.z);
        if (s > 0)
        {
            x = q.x / s;
            y = q.y / s;
            z = q.z / s;

        }
        else
        {
            x = q.x;
            y = q.y;
            z = q.z;
        }
        double angle = 2 * Math.acos(q.w);
        return new Point(x, y, z, angle);
    }

    /**
     * @param a
     * @param b
     * @return
     * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
     */
    public static Quaternion Add(Quaternion a, Quaternion b)
    {
        if (a == null || b == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
    }

    /**
     * @param a
     * @param b
     * @return
     * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
     */
    public static Quaternion Subtract(Quaternion a, Quaternion b)
    {
        if (a == null || b == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return new Quaternion(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
    }

    /**
     * @param a
     * @param b
     * @return
     * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
     */
    public static Quaternion Multiply(Quaternion a, Quaternion b)
    {
        if (a == null || b == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return new Quaternion(
            a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
            a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
            a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
            a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);
    }

    /**
     * @param q
     * @return
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static Quaternion Multiply(double s, Quaternion q)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return new Quaternion(s * q.x, s * q.y, s * q.z, s * q.w);
    }

    /**
     * @param q
     * @param s
     * @return
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static Quaternion Multiply(Quaternion q, double s)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return new Quaternion(s * q.x, s * q.y, s * q.z, s * q.w);
    }

    /**
     * equivalent to multiplying by the quaternion (0, v)
     *
     * @param v
     * @param q
     * @return
     * @throws IllegalArgumentException if <code>q</code> or <code>v</code> is null
     */
    public static Quaternion Multiply(Point v, Quaternion q)
    {
        if (v == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.VectorIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return new Quaternion(
            v.x() * q.w + v.y() * q.z - v.z() * q.y,
            v.y() * q.w + v.z() * q.x - v.x() * q.z,
            v.z() * q.w + v.x() * q.y - v.y() * q.x,
            -v.x() * q.x - v.y() * q.y - v.z() * q.z);
    }

    /**
     * @param q
     * @param s
     * @return
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static Quaternion Divide(Quaternion q, double s)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return Quaternion.Multiply(q, (1 / s));
    }

    // conjugate operator
    public Quaternion Conjugate()
    {
        return new Quaternion(-x, -y, -z, w);
    }

    /**
     * @param q
     * @return
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static double Norm2(Quaternion q)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
    }

    /**
     * @param q
     * @return
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static double Abs(Quaternion q)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return Math.sqrt(Norm2(q));
    }

    /**
     * @param a
     * @param b
     * @return
     * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
     */
    public static Quaternion Divide(Quaternion a, Quaternion b)
    {
        if (a == null || b == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return Quaternion.Multiply(a, Quaternion.Divide(b.Conjugate(), Abs(b)));
    }

    /**
     * @param a
     * @param b
     * @return
     * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
     */
    public static double Dot(Quaternion a, Quaternion b)
    {
        if (a == null || b == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
    }

    public Quaternion Normalize()
    {
        double L = this.Length();

        return new Quaternion(
            x / L,
            y / L,
            z / L,
            w / L);
    }

    public double Length()
    {
        return Math.sqrt(
            x * x + y * y + z * z + w * w);
    }

    /**
     * @param q0
     * @param q1
     * @param t
     * @return
     * @throws IllegalArgumentException if either supplied <code>Quaternion</code> is null
     */
    public static Quaternion Slerp(Quaternion q0, Quaternion q1, double t)
    {
        if (q0 == null || q1 == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        double cosom = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
        double tmp0, tmp1, tmp2, tmp3;
        if (cosom < 0.0)
        {
            cosom = -cosom;
            tmp0 = -q1.x;
            tmp1 = -q1.y;
            tmp2 = -q1.z;
            tmp3 = -q1.w;
        }
        else
        {
            tmp0 = q1.x;
            tmp1 = q1.y;
            tmp2 = q1.z;
            tmp3 = q1.w;
        }

        /* calc coeffs */
        double scale0, scale1;

        if ((1.0 - cosom) > epsilon)
        {
            // standard case (slerp)
            double omega = Math.acos(cosom);
            double sinom = Math.sin(omega);
            scale0 = Math.sin((1.0 - t) * omega) / sinom;
            scale1 = Math.sin(t * omega) / sinom;
        }
        else
        {
            /* just lerp */
            scale0 = 1.0 - t;
            scale1 = t;
        }

        return new Quaternion(
            scale0 * q0.x + scale1 * tmp0,
            scale0 * q0.y + scale1 * tmp1,
            scale0 * q0.z + scale1 * tmp2,
            scale0 * q0.w + scale1 * tmp3);
    }

    public Quaternion Ln()
    {
        return Ln(this);
    }

    /**
     * @param q
     * @return
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static Quaternion Ln(Quaternion q)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        double t;

        double s = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
        double om = Math.atan2(s, q.w);

        if (Math.abs(s) < epsilon)
            t = 0.0f;
        else
            t = om / s;

        return new Quaternion(q.x * t, q.y * t, q.z * t, 0.0f);
    }

    /*****************the below functions have not been certified to work properly ******************/

    /**
     * @param q
     * @return
     * @throws IllegalArgumentException if <code>q</code> is null
     */
    public static Quaternion Exp(Quaternion q)
    {
        if (q == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        double sinom;
        double om = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);

        if (Math.abs(om) < epsilon)
            sinom = 1.0;
        else
            sinom = Math.sin(om) / om;

        return new Quaternion(q.x * sinom, q.y * sinom, q.z * sinom, Math.cos(om));
    }

    public Quaternion Exp()
    {
        return Ln(this);
    }

    /**
     * @param q1
     * @param a
     * @param b
     * @param c
     * @param t
     * @return
     * @throws IllegalArgumentException if any argument is null
     */
    public static Quaternion Squad(
        Quaternion q1,
        Quaternion a,
        Quaternion b,
        Quaternion c,
        double t)
    {
        if (q1 == null || a == null || b == null || c == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }
        return Slerp(
            Slerp(q1, c, t), Slerp(a, b, t), 2 * t * (1.0 - t));
    }

    //TODO: this needs to be accounted for before Squad() is used
    /*public static Quaternion[] SquadSetup(
        Quaternion4d q0,
        Quaternion4d q1,
        Quaternion4d q2,
        Quaternion4d q3)
    {
        if(q0 == null || q1 == null || q2 == null || q3 == null)
        {
            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
            gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
            throw new IllegalArgumentException(msg);
        }

        q0 = q0 + q1;
        q0.Normalize();

        q2 = q2 + q1;
        q2.Normalize();

        q3 = q3 + q1;
        q3.Normalize();

        q1.Normalize();

        Quaternion[] ret = new Quaternion[3];

        ret[0] = q1 * Exp(-0.25 * (Ln(Exp(q1) * q2) + Ln(Exp(q1) * q0))); // outA
        ret[1] = q2 * Exp(-0.25 * (Ln(Exp(q2) * q3) + Ln(Exp(q2) * q1))); // outB
        ret[2] = q2;                                                      // outC

    }*/
}
