package com.flowpowered.math.imaginary;

import java.io.Serializable;

import com.flowpowered.math.GenericMath;
import com.flowpowered.math.HashFunctions;
import com.flowpowered.math.TrigMath;
import com.flowpowered.math.matrix.Matrix3f;
import com.flowpowered.math.vector.Vector3f;

/**
 * Represent a quaternion of the form xi + yj + zk + w. The x, y, z and w components are stored as floats. This class is immutable.
 */
public class Quaternionf implements Imaginaryf, Comparable<Quaternionf>, Serializable, Cloneable {
    private static final long serialVersionUID = 1;
    /**
     * An immutable identity (0, 0, 0, 0) quaternion.
     */
    public static final Quaternionf ZERO = new Quaternionf(0, 0, 0, 0);
    /**
     * An immutable identity (0, 0, 0, 1) quaternion.
     */
    public static final Quaternionf IDENTITY = new Quaternionf(0, 0, 0, 1);
    private final float x;
    private final float y;
    private final float z;
    private final float w;
    private transient volatile boolean hashed = false;
    private transient volatile int hashCode = 0;

    /**
     * Constructs a new quaternion. The components are set to the identity (0, 0, 0, 1).
     */
    public Quaternionf() {
        this(0, 0, 0, 1);
    }

    /**
     * Constructs a new quaternion from the double components.
     *
     * @param x The x (imaginary) component
     * @param y The y (imaginary) component
     * @param z The z (imaginary) component
     * @param w The w (real) component
     */
    public Quaternionf(double x, double y, double z, double w) {
        this((float) x, (float) y, (float) z, (float) w);
    }

    /**
     * Constructs a new quaternion from the float components.
     *
     * @param x The x (imaginary) component
     * @param y The y (imaginary) component
     * @param z The z (imaginary) component
     * @param w The w (real) component
     */
    public Quaternionf(float x, float y, float z, float w) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }

    /**
     * Copy constructor.
     *
     * @param q The quaternion to copy
     */
    public Quaternionf(Quaternionf q) {
        this(q.x, q.y, q.z, q.w);
    }

    /**
     * Gets the x (imaginary) component of this quaternion.
     *
     * @return The x (imaginary) component
     */
    public float getX() {
        return x;
    }

    /**
     * Gets the y (imaginary) component of this quaternion.
     *
     * @return The y (imaginary) component
     */
    public float getY() {
        return y;
    }

    /**
     * Gets the z (imaginary) component of this quaternion.
     *
     * @return The z (imaginary) component
     */
    public float getZ() {
        return z;
    }

    /**
     * Gets the w (real) component of this quaternion.
     *
     * @return The w (real) component
     */
    public float getW() {
        return w;
    }

    /**
     * Adds another quaternion to this one.
     *
     * @param q The quaternion to add
     * @return A new quaternion, which is the sum of both
     */
    public Quaternionf add(Quaternionf q) {
        return add(q.x, q.y, q.z, q.w);
    }

    /**
     * Adds the double components of another quaternion to this one.
     *
     * @param x The x (imaginary) component of the quaternion to add
     * @param y The y (imaginary) component of the quaternion to add
     * @param z The z (imaginary) component of the quaternion to add
     * @param w The w (real) component of the quaternion to add
     * @return A new quaternion, which is the sum of both
     */
    public Quaternionf add(double x, double y, double z, double w) {
        return add((float) x, (float) y, (float) z, (float) w);
    }

    /**
     * Adds the float components of another quaternion to this one.
     *
     * @param x The x (imaginary) component of the quaternion to add
     * @param y The y (imaginary) component of the quaternion to add
     * @param z The z (imaginary) component of the quaternion to add
     * @param w The w (real) component of the quaternion to add
     * @return A new quaternion, which is the sum of both
     */
    public Quaternionf add(float x, float y, float z, float w) {
        return new Quaternionf(this.x + x, this.y + y, this.z + z, this.w + w);
    }

    /**
     * Subtracts another quaternion from this one.
     *
     * @param q The quaternion to subtract
     * @return A new quaternion, which is the difference of both
     */
    public Quaternionf sub(Quaternionf q) {
        return sub(q.x, q.y, q.z, q.w);
    }

    /**
     * Subtracts the double components of another quaternion from this one.
     *
     * @param x The x (imaginary) component of the quaternion to subtract
     * @param y The y (imaginary) component of the quaternion to subtract
     * @param z The z (imaginary) component of the quaternion to subtract
     * @param w The w (real) component of the quaternion to subtract
     * @return A new quaternion, which is the difference of both
     */
    public Quaternionf sub(double x, double y, double z, double w) {
        return sub((float) x, (float) y, (float) z, (float) w);
    }

    /**
     * Subtracts the float components of another quaternion from this one.
     *
     * @param x The x (imaginary) component of the quaternion to subtract
     * @param y The y (imaginary) component of the quaternion to subtract
     * @param z The z (imaginary) component of the quaternion to subtract
     * @param w The w (real) component of the quaternion to subtract
     * @return A new quaternion, which is the difference of both
     */
    public Quaternionf sub(float x, float y, float z, float w) {
        return new Quaternionf(this.x - x, this.y - y, this.z - z, this.w - w);
    }

    /**
     * Multiplies the components of this quaternion by a double scalar.
     *
     * @param a The multiplication scalar
     * @return A new quaternion, which has each component multiplied by the scalar
     */
    public Quaternionf mul(double a) {
        return mul((float) a);
    }

    /**
     * Multiplies the components of this quaternion by a float scalar.
     *
     * @param a The multiplication scalar
     * @return A new quaternion, which has each component multiplied by the scalar
     */
    @Override
    public Quaternionf mul(float a) {
        return new Quaternionf(x * a, y * a, z * a, w * a);
    }

    /**
     * Multiplies another quaternion with this one.
     *
     * @param q The quaternion to multiply with
     * @return A new quaternion, which is the product of both
     */
    public Quaternionf mul(Quaternionf q) {
        return mul(q.x, q.y, q.z, q.w);
    }

    /**
     * Multiplies the double components of another quaternion with this one.
     *
     * @param x The x (imaginary) component of the quaternion to multiply with
     * @param y The y (imaginary) component of the quaternion to multiply with
     * @param z The z (imaginary) component of the quaternion to multiply with
     * @param w The w (real) component of the quaternion to multiply with
     * @return A new quaternion, which is the product of both
     */
    public Quaternionf mul(double x, double y, double z, double w) {
        return mul((float) x, (float) y, (float) z, (float) w);
    }

    /**
     * Multiplies the float components of another quaternion with this one.
     *
     * @param x The x (imaginary) component of the quaternion to multiply with
     * @param y The y (imaginary) component of the quaternion to multiply with
     * @param z The z (imaginary) component of the quaternion to multiply with
     * @param w The w (real) component of the quaternion to multiply with
     * @return A new quaternion, which is the product of both
     */
    public Quaternionf mul(float x, float y, float z, float w) {
        return new Quaternionf(
                this.w * x + this.x * w + this.y * z - this.z * y,
                this.w * y + this.y * w + this.z * x - this.x * z,
                this.w * z + this.z * w + this.x * y - this.y * x,
                this.w * w - this.x * x - this.y * y - this.z * z);
    }

    /**
     * Divides the components of this quaternion by a double scalar.
     *
     * @param a The division scalar
     * @return A new quaternion, which has each component divided by the scalar
     */
    public Quaternionf div(double a) {
        return div((float) a);
    }

    /**
     * Divides the components of this quaternion by a float scalar.
     *
     * @param a The division scalar
     * @return A new quaternion, which has each component divided by the scalar
     */
    @Override
    public Quaternionf div(float a) {
        return new Quaternionf(x / a, y / a, z / a, w / a);
    }

    /**
     * Divides this quaternions by another one.
     *
     * @param q The quaternion to divide with
     * @return The quotient of the two quaternions
     */
    public Quaternionf div(Quaternionf q) {
        return div(q.x, q.y, q.z, q.w);
    }

    /**
     * Divides this quaternions by the double components of another one.
     *
     * @param x The x (imaginary) component of the quaternion to divide with
     * @param y The y (imaginary) component of the quaternion to divide with
     * @param z The z (imaginary) component of the quaternion to divide with
     * @param w The w (real) component of the quaternion to divide with
     * @return The quotient of the two quaternions
     */
    public Quaternionf div(double x, double y, double z, double w) {
        return div((float) x, (float) y, (float) z, (float) w);
    }

    /**
     * Divides this quaternions by the float components of another one.
     *
     * @param x The x (imaginary) component of the quaternion to divide with
     * @param y The y (imaginary) component of the quaternion to divide with
     * @param z The z (imaginary) component of the quaternion to divide with
     * @param w The w (real) component of the quaternion to divide with
     * @return The quotient of the two quaternions
     */
    public Quaternionf div(float x, float y, float z, float w) {
        final float d = x * x + y * y + z * z + w * w;
        return new Quaternionf(
                (this.x * w - this.w * x - this.z * y + this.y * z) / d,
                (this.y * w + this.z * x - this.w * y - this.x * z) / d,
                (this.z * w - this.y * x + this.x * y - this.w * z) / d,
                (this.w * w + this.x * x + this.y * y + this.z * z) / d);
    }

    /**
     * Returns the dot product of this quaternion with another one.
     *
     * @param q The quaternion to calculate the dot product with
     * @return The dot product of the two quaternions
     */
    public float dot(Quaternionf q) {
        return dot(q.x, q.y, q.z, q.w);
    }

    /**
     * Returns the dot product of this quaternion with the double components of another one.
     *
     * @param x The x (imaginary) component of the quaternion to calculate the dot product with
     * @param y The y (imaginary) component of the quaternion to calculate the dot product with
     * @param z The z (imaginary) component of the quaternion to calculate the dot product with
     * @param w The w (real) component of the quaternion to calculate the dot product with
     * @return The dot product of the two quaternions
     */
    public float dot(double x, double y, double z, double w) {
        return dot((float) x, (float) y, (float) z, (float) w);
    }

    /**
     * Returns the dot product of this quaternion with the float components of another one.
     *
     * @param x The x (imaginary) component of the quaternion to calculate the dot product with
     * @param y The y (imaginary) component of the quaternion to calculate the dot product with
     * @param z The z (imaginary) component of the quaternion to calculate the dot product with
     * @param w The w (real) component of the quaternion to calculate the dot product with
     * @return The dot product of the two quaternions
     */
    public float dot(float x, float y, float z, float w) {
        return this.x * x + this.y * y + this.z * z + this.w * w;
    }

    /**
     * Rotates a vector by this quaternion.
     *
     * @param v The vector to rotate
     * @return The rotated vector
     */
    public Vector3f rotate(Vector3f v) {
        return rotate(v.getX(), v.getY(), v.getZ());
    }

    /**
     * Rotates the double components of a vector by this quaternion.
     *
     * @param x The x component of the vector
     * @param y The y component of the vector
     * @param z The z component of the vector
     * @return The rotated vector
     */
    public Vector3f rotate(double x, double y, double z) {
        return rotate((float) x, (float) y, (float) z);
    }

    /**
     * Rotates the float components of a vector by this quaternion.
     *
     * @param x The x component of the vector
     * @param y The y component of the vector
     * @param z The z component of the vector
     * @return The rotated vector
     */
    public Vector3f rotate(float x, float y, float z) {
        final float length = length();
        if (Math.abs(length) < GenericMath.FLT_EPSILON) {
            throw new ArithmeticException("Cannot rotate by the zero quaternion");
        }
        final float nx = this.x / length;
        final float ny = this.y / length;
        final float nz = this.z / length;
        final float nw = this.w / length;
        final float px = nw * x + ny * z - nz * y;
        final float py = nw * y + nz * x - nx * z;
        final float pz = nw * z + nx * y - ny * x;
        final float pw = -nx * x - ny * y - nz * z;
        return new Vector3f(
                pw * -nx + px * nw - py * nz + pz * ny,
                pw * -ny + py * nw - pz * nx + px * nz,
                pw * -nz + pz * nw - px * ny + py * nx);
    }

    /**
     * Returns a unit vector representing the direction of this quaternion, which is {@link Vector3f#FORWARD} rotated by this quaternion.
     *
     * @return The vector representing the direction this quaternion is pointing to
     */
    public Vector3f getDirection() {
        return rotate(Vector3f.FORWARD);
    }

    /**
     * Returns the axis of rotation for this quaternion.
     *
     * @return The axis of rotation
     */
    public Vector3f getAxis() {
        final float q = (float) Math.sqrt(1 - w * w);
        return new Vector3f(x / q, y / q, z / q);
    }

    /**
     * Returns the angles in degrees around the x, y and z axes that correspond to the rotation represented by this quaternion.
     *
     * @return The angle in degrees for each axis, stored in a vector, in the corresponding component
     */
    public Vector3f getAxesAnglesDeg() {
        return getAxesAnglesRad().mul(TrigMath.RAD_TO_DEG);
    }

    /**
     * Returns the angles in radians around the x, y and z axes that correspond to the rotation represented by this quaternion.
     *
     * @return The angle in radians for each axis, stored in a vector, in the corresponding component
     */
    public Vector3f getAxesAnglesRad() {
        final double roll;
        final double pitch;
        double yaw;
        final double test = w * x - y * z;
        if (Math.abs(test) < 0.4999) {
            roll = TrigMath.atan2(2 * (w * z + x * y), 1 - 2 * (x * x + z * z));
            pitch = TrigMath.asin(2 * test);
            yaw = TrigMath.atan2(2 * (w * y + z * x), 1 - 2 * (x * x + y * y));
        } else {
            final int sign = (test < 0) ? -1 : 1;
            roll = 0;
            pitch = sign * Math.PI / 2;
            yaw = -sign * 2 * TrigMath.atan2(z, w);
        }
        return new Vector3f(pitch, yaw, roll);
    }

    /**
     * Conjugates the quaternion.
 Conjugation of a quaternion a is an operation returning quaternion a' such that a' * a = a * a' = |a|2 where
     * |a|2 is squared length of a.
     *
     * @return the conjugated quaternion
     */
    @Override
    public Quaternionf conjugate() {
        return new Quaternionf(-x, -y, -z, w);
    }

    /**
     * Inverts the quaternion.
 Inversion of a quaternion a returns quaternion a-1 = a' / |a|2 where a' is {@link #conjugate()
     * conjugation} of a, and |a|2 is squared length of a.
 For any quaternions a, b, c, such that a * b = c equations
     * a-1 * c = b and c * b-1 = a are true.
     *
     * @return the inverted quaternion
     */
    @Override
    public Quaternionf invert() {
        final float lengthSquared = lengthSquared();
        if (Math.abs(lengthSquared) < GenericMath.FLT_EPSILON) {
            throw new ArithmeticException("Cannot invert a quaternion of length zero");
        }
        return conjugate().div(lengthSquared);
    }

    /**
     * Returns the square of the length of this quaternion.
     *
     * @return The square of the length
     */
    @Override
    public float lengthSquared() {
        return x * x + y * y + z * z + w * w;
    }

    /**
     * Returns the length of this quaternion.
     *
     * @return The length
     */
    @Override
    public float length() {
        return (float) Math.sqrt(lengthSquared());
    }

    /**
     * Normalizes this quaternion.
     *
     * @return A new quaternion of unit length
     */
    @Override
    public Quaternionf normalize() {
        final float length = length();
        if (Math.abs(length) < GenericMath.FLT_EPSILON) {
            throw new ArithmeticException("Cannot normalize the zero quaternion");
        }
        return new Quaternionf(x / length, y / length, z / length, w / length);
    }

    /**
     * Converts this quaternion to a complex by extracting the rotation around
     * the axis and returning it as a rotation in the plane perpendicular to the
     * rotation axis.
     *
     * @return The rotation without the axis as a complex
     */
    public Complexf toComplex() {
        final float w2 = w * w;
        return new Complexf(2 * w2 - 1, 2 * w * (float) Math.sqrt(1 - w2));
    }

    @Override
    public Quaternionf toFloat() {
        return new Quaternionf(x, y, z, w);
    }

    @Override
    public Quaterniond toDouble() {
        return new Quaterniond(x, y, z, w);
    }

   @Override
    public boolean equals(Object o) {
        if (this == o) {
            return true;
        }
        if (!(o instanceof Quaternionf)) {
            return false;
        }
        final Quaternionf quaternion = (Quaternionf) o;
        if (Float.compare(quaternion.w, w) != 0) {
            return false;
        }
        if (Float.compare(quaternion.x, x) != 0) {
            return false;
        }
        if (Float.compare(quaternion.y, y) != 0) {
            return false;
        }
        if (Float.compare(quaternion.z, z) != 0) {
            return false;
        }
        return true;
    }

    @Override
    public int hashCode() {
        if (!hashed) {
            int result = (x != +0.0f ? HashFunctions.hash(x) : 0);
            result = 31 * result + (y != +0.0f ? HashFunctions.hash(y) : 0);
            result = 31 * result + (z != +0.0f ? HashFunctions.hash(z) : 0);
            hashCode = 31 * result + (w != +0.0f ? HashFunctions.hash(w) : 0);
            hashed = true;
        }
        return hashCode;
    }

    @Override
    public int compareTo(Quaternionf q) {
        return (int) Math.signum(lengthSquared() - q.lengthSquared());
    }

    @Override
    public Quaternionf clone() {
        return new Quaternionf(this);
    }

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

    /**
     * Creates a new quaternion from the float real component.
     *
     *
The {@link #ZERO} constant is re-used when {@code w} is 0.

     *
     * @param w The w (real) component
     * @return The quaternion created from the float real component
     */
    public static Quaternionf fromReal(float w) {
         return w == 0 ? ZERO : new Quaternionf(0, 0, 0, w);
    }

    /**
     * Creates a new quaternion from the float imaginary components.
     *
     *
The {@link #ZERO} constant is re-used when {@code x}, {@code y}, and {@code z} are 0.

     *
     * @param x The x (imaginary) component
     * @param y The y (imaginary) component
     * @param z The z (imaginary) component
     * @return The quaternion created from the float imaginary components
     */
    public static Quaternionf fromImaginary(float x, float y, float z) {
         return x == 0 && y == 0 && z == 0 ? ZERO : new Quaternionf(x, y, z, 0);
    }

    /**
     * Creates a new quaternion from the float components.
     *
     *
The {@link #ZERO} constant is re-used when {@code x}, {@code y}, {@code z}, and {@code w} are 0.

     *
     * @param x The x (imaginary) component
     * @param y The y (imaginary) component
     * @param z The z (imaginary) component
     * @param w The w (real) component
     * @return The quaternion created from the float components
     */
    public static Quaternionf from(float x, float y, float z, float w) {
         return x == 0 && y == 0 && z == 0 && w == 0 ? ZERO : new Quaternionf(x, y, z, w);
    }

    /**
     * Creates a new quaternion from the double angles in degrees around the x, y and z axes.
     *
     * @param pitch The rotation around x
     * @param yaw The rotation around y
     * @param roll The rotation around z
     * @return The quaternion defined by the rotations around the axes
     */
    public static Quaternionf fromAxesAnglesDeg(double pitch, double yaw, double roll) {
        return fromAxesAnglesDeg((float) pitch, (float) yaw, (float) roll);
    }

    /**
     * Creates a new quaternion from the double angles in radians around the x, y and z axes.
     *
     * @param pitch The rotation around x
     * @param yaw The rotation around y
     * @param roll The rotation around z
     * @return The quaternion defined by the rotations around the axes
     */
    public static Quaternionf fromAxesAnglesRad(double pitch, double yaw, double roll) {
        return fromAxesAnglesRad((float) pitch, (float) yaw, (float) roll);
    }

    /**
     * Creates a new quaternion from the float angles in degrees around the x, y and z axes.
     *
     * @param pitch The rotation around x
     * @param yaw The rotation around y
     * @param roll The rotation around z
     * @return The quaternion defined by the rotations around the axes
     */
    public static Quaternionf fromAxesAnglesDeg(float pitch, float yaw, float roll) {
        return Quaternionf.fromAngleDegAxis(yaw, Vector3f.UNIT_Y).
                mul(Quaternionf.fromAngleDegAxis(pitch, Vector3f.UNIT_X)).
                mul(Quaternionf.fromAngleDegAxis(roll, Vector3f.UNIT_Z));
    }

    /**
     * Creates a new quaternion from the float angles in radians around the x, y and z axes.
     *
     * @param pitch The rotation around x
     * @param yaw The rotation around y
     * @param roll The rotation around z
     * @return The quaternion defined by the rotations around the axes
     */
    public static Quaternionf fromAxesAnglesRad(float pitch, float yaw, float roll) {
        return Quaternionf.fromAngleRadAxis(yaw, Vector3f.UNIT_Y).
                mul(Quaternionf.fromAngleRadAxis(pitch, Vector3f.UNIT_X)).
                mul(Quaternionf.fromAngleRadAxis(roll, Vector3f.UNIT_Z));
    }

    /**
     * Creates a new quaternion from the angle-axis rotation defined from the first to the second vector.
     *
     * @param from The first vector
     * @param to The second vector
     * @return The quaternion defined by the angle-axis rotation between the vectors
     */
    public static Quaternionf fromRotationTo(Vector3f from, Vector3f to) {
        return Quaternionf.fromAngleRadAxis(TrigMath.acos(from.dot(to) / (from.length() * to.length())), from.cross(to));
    }

    /**
     * Creates a new quaternion from the rotation double angle in degrees around the axis vector.
     *
     * @param angle The rotation angle in degrees
     * @param axis The axis of rotation
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleDegAxis(double angle, Vector3f axis) {
        return fromAngleRadAxis(Math.toRadians(angle), axis);
    }

    /**
     * Creates a new quaternion from the rotation double angle in radians around the axis vector.
     *
     * @param angle The rotation angle in radians
     * @param axis The axis of rotation
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleRadAxis(double angle, Vector3f axis) {
        return fromAngleRadAxis((float) angle, axis);
    }

    /**
     * Creates a new quaternion from the rotation float angle in degrees around the axis vector.
     *
     * @param angle The rotation angle in degrees
     * @param axis The axis of rotation
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleDegAxis(float angle, Vector3f axis) {
        return fromAngleRadAxis((float) Math.toRadians(angle), axis);
    }

    /**
     * Creates a new quaternion from the rotation float angle in radians around the axis vector.
     *
     * @param angle The rotation angle in radians
     * @param axis The axis of rotation
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleRadAxis(float angle, Vector3f axis) {
        return fromAngleRadAxis(angle, axis.getX(), axis.getY(), axis.getZ());
    }

    /**
     * Creates a new quaternion from the rotation double angle in degrees around the axis vector double components.
     *
     * @param angle The rotation angle in degrees
     * @param x The x component of the axis vector
     * @param y The y component of the axis vector
     * @param z The z component of the axis vector
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleDegAxis(double angle, double x, double y, double z) {
        return fromAngleRadAxis(Math.toRadians(angle), x, y, z);
    }

    /**
     * Creates a new quaternion from the rotation double angle in radians around the axis vector double components.
     *
     * @param angle The rotation angle in radians
     * @param x The x component of the axis vector
     * @param y The y component of the axis vector
     * @param z The z component of the axis vector
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleRadAxis(double angle, double x, double y, double z) {
        return fromAngleRadAxis((float) angle, (float) x, (float) y, (float) z);
    }

    /**
     * Creates a new quaternion from the rotation float angle in degrees around the axis vector float components.
     *
     * @param angle The rotation angle in degrees
     * @param x The x component of the axis vector
     * @param y The y component of the axis vector
     * @param z The z component of the axis vector
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleDegAxis(float angle, float x, float y, float z) {
        return fromAngleRadAxis((float) Math.toRadians(angle), x, y, z);
    }

    /**
     * Creates a new quaternion from the rotation float angle in radians around the axis vector float components.
     *
     * @param angle The rotation angle in radians
     * @param x The x component of the axis vector
     * @param y The y component of the axis vector
     * @param z The z component of the axis vector
     * @return The quaternion defined by the rotation around the axis
     */
    public static Quaternionf fromAngleRadAxis(float angle, float x, float y, float z) {
        final float halfAngle = angle / 2;
        final float q = TrigMath.sin(halfAngle) / (float) Math.sqrt(x * x + y * y + z * z);
        return new Quaternionf(x * q, y * q, z * q, TrigMath.cos(halfAngle));
    }

    /**
     * Creates a new quaternion from the rotation matrix. The matrix will be interpreted as a rotation matrix even if it is not.
     *
     * @param matrix The rotation matrix
     * @return The quaternion defined by the rotation matrix
     */
    public static Quaternionf fromRotationMatrix(Matrix3f matrix) {
        final float trace = matrix.trace();
        if (trace < 0) {
            if (matrix.get(1, 1) > matrix.get(0, 0)) {
                if (matrix.get(2, 2) > matrix.get(1, 1)) {
                    final float r = (float) Math.sqrt(matrix.get(2, 2) - matrix.get(0, 0) - matrix.get(1, 1) + 1);
                    final float s = 0.5f / r;
                    return new Quaternionf(
                            (matrix.get(2, 0) + matrix.get(0, 2)) * s,
                            (matrix.get(1, 2) + matrix.get(2, 1)) * s,
                            0.5f * r,
                            (matrix.get(1, 0) - matrix.get(0, 1)) * s);
                } else {
                    final float r = (float) Math.sqrt(matrix.get(1, 1) - matrix.get(2, 2) - matrix.get(0, 0) + 1);
                    final float s = 0.5f / r;
                    return new Quaternionf(
                            (matrix.get(0, 1) + matrix.get(1, 0)) * s,
                            0.5f * r,
                            (matrix.get(1, 2) + matrix.get(2, 1)) * s,
                            (matrix.get(0, 2) - matrix.get(2, 0)) * s);
                }
            } else if (matrix.get(2, 2) > matrix.get(0, 0)) {
                final float r = (float) Math.sqrt(matrix.get(2, 2) - matrix.get(0, 0) - matrix.get(1, 1) + 1);
                final float s = 0.5f / r;
                return new Quaternionf(
                        (matrix.get(2, 0) + matrix.get(0, 2)) * s,
                        (matrix.get(1, 2) + matrix.get(2, 1)) * s,
                        0.5f * r,
                        (matrix.get(1, 0) - matrix.get(0, 1)) * s);
            } else {
                final float r = (float) Math.sqrt(matrix.get(0, 0) - matrix.get(1, 1) - matrix.get(2, 2) + 1);
                final float s = 0.5f / r;
                return new Quaternionf(
                        0.5f * r,
                        (matrix.get(0, 1) + matrix.get(1, 0)) * s,
                        (matrix.get(2, 0) - matrix.get(0, 2)) * s,
                        (matrix.get(2, 1) - matrix.get(1, 2)) * s);
            }
        } else {
            final float r = (float) Math.sqrt(trace + 1);
            final float s = 0.5f / r;
            return new Quaternionf(
                    (matrix.get(2, 1) - matrix.get(1, 2)) * s,
                    (matrix.get(0, 2) - matrix.get(2, 0)) * s,
                    (matrix.get(1, 0) - matrix.get(0, 1)) * s,
                    0.5f * r);
        }
    }
}


package com.flowpowered.math.matrix;

import java.io.Serializable;
import java.util.Arrays;

import com.flowpowered.math.GenericMath;
import com.flowpowered.math.TrigMath;
import com.flowpowered.math.imaginary.Complexf;
import com.flowpowered.math.imaginary.Quaternionf;
import com.flowpowered.math.vector.Vector3f;
import com.flowpowered.math.vector.VectorNf;

public class MatrixNf implements Matrixf, Serializable, Cloneable {
    private static final long serialVersionUID = 1;
    public static final MatrixNf IDENTITY_2 = new ImmutableIdentityMatrixN(2);
    public static final MatrixNf IDENTITY_3 = new ImmutableIdentityMatrixN(3);
    public static final MatrixNf IDENTITY_4 = new ImmutableIdentityMatrixN(4);
    private final float[][] mat;

    public MatrixNf(int size) {
        if (size < 2) {
            throw new IllegalArgumentException("Minimum matrix size is 2");
        }
        mat = new float[size][size];
        setIdentity();
    }

    public MatrixNf(Matrix2f m) {
        mat = new float[][]{
                {m.get(0, 0), m.get(0, 1)},
                {m.get(1, 0), m.get(1, 1)}
        };
    }

    public MatrixNf(Matrix3f m) {
        mat = new float[][]{
                {m.get(0, 0), m.get(0, 1), m.get(0, 2)},
                {m.get(1, 0), m.get(1, 1), m.get(1, 2)},
                {m.get(2, 0), m.get(2, 1), m.get(2, 2)}
        };
    }

    public MatrixNf(Matrix4f m) {
        mat = new float[][]{
                {m.get(0, 0), m.get(0, 1), m.get(0, 2), m.get(0, 3)},
                {m.get(1, 0), m.get(1, 1), m.get(1, 2), m.get(1, 3)},
                {m.get(2, 0), m.get(2, 1), m.get(2, 2), m.get(2, 3)},
                {m.get(3, 0), m.get(3, 1), m.get(3, 2), m.get(3, 3)}
        };
    }

    public MatrixNf(float... m) {
        if (m.length < 4) {
            throw new IllegalArgumentException("Minimum matrix size is 2");
        }
        final int size = (int) Math.ceil(Math.sqrt(m.length));
        mat = new float[size][size];
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                final int index = col + row * size;
                if (index < m.length) {
                    mat[row][col] = m[index];
                } else {
                    mat[row][col] = 0;
                }
            }
        }
    }

    public MatrixNf(MatrixNf m) {
        mat = deepClone(m.mat);
    }

    public int size() {
        return mat.length;
    }

    @Override
    public float get(int row, int col) {
        return mat[row][col];
    }

    @Override
    public VectorNf getRow(int row) {
        final int size = size();
        final VectorNf d = new VectorNf(size);
        for (int col = 0; col < size; col++) {
            d.set(col, get(row, col));
        }
        return d;
    }

    @Override
    public VectorNf getColumn(int col) {
        final int size = size();
        final VectorNf d = new VectorNf(size);
        for (int row = 0; row < size; row++) {
            d.set(row, get(row, col));
        }
        return d;
    }

    public void set(int row, int col, double val) {
        set(row, col, (float) val);
    }

    public void set(int row, int col, float val) {
        mat[row][col] = val;
    }

    public final void setIdentity() {
        final int size = size();
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                if (row == col) {
                    mat[row][col] = 1;
                } else {
                    mat[row][col] = 0;
                }
            }
        }
    }

    public void setZero() {
        final int size = size();
        for (int row = 0; row < size; row++) {
            Arrays.fill(mat[row], 0);
        }
    }

    public MatrixNf resize(int size) {
        final MatrixNf d = new MatrixNf(size);
        for (int rowCol = size(); rowCol < size; rowCol++) {
            d.set(rowCol, rowCol, 0);
        }
        size = Math.min(size, size());
        for (int row = 0; row < size; row++) {
            System.arraycopy(mat[row], 0, d.mat[row], 0, size);
        }
        return d;
    }

    public MatrixNf add(MatrixNf m) {
        final int size = size();
        if (size != m.size()) {
            throw new IllegalArgumentException("Matrix sizes must be the same");
        }
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = mat[row][col] + m.mat[row][col];
            }
        }
        return d;
    }

    public MatrixNf sub(MatrixNf m) {
        final int size = size();
        if (size != m.size()) {
            throw new IllegalArgumentException("Matrix sizes must be the same");
        }
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = mat[row][col] - m.mat[row][col];
            }
        }
        return d;
    }

    public MatrixNf mul(double a) {
        return mul((float) a);
    }

    @Override
    public MatrixNf mul(float a) {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = mat[row][col] * a;
            }
        }
        return d;
    }

    public MatrixNf mul(MatrixNf m) {
        final int size = size();
        if (size != m.size()) {
            throw new IllegalArgumentException("Matrix sizes must be the same");
        }
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                float dot = 0;
                for (int i = 0; i < size; i++) {
                    dot += mat[row][i] * m.mat[i][col];
                }
                d.mat[row][col] = dot;
            }
        }
        return d;
    }

    public MatrixNf div(double a) {
        return div((float) a);
    }

    @Override
    public MatrixNf div(float a) {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = mat[row][col] / a;
            }
        }
        return d;
    }

    public MatrixNf div(MatrixNf m) {
        return mul(m.invert());
    }

    public MatrixNf pow(double pow) {
        return pow((float) pow);
    }

    @Override
    public MatrixNf pow(float pow) {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = (float) Math.pow(mat[row][col], pow);
            }
        }
        return d;
    }

    public MatrixNf translate(VectorNf v) {
        return translate(v.toArray());
    }

    public MatrixNf translate(float... v) {
        return createTranslation(v).mul(this);
    }

    public MatrixNf scale(VectorNf v) {
        return scale(v.toArray());
    }

    public MatrixNf scale(float... v) {
        return createScaling(v).mul(this);
    }

    public MatrixNf rotate(Complexf rot) {
        return createRotation(size(), rot).mul(this);
    }

    public MatrixNf rotate(Quaternionf rot) {
        return createRotation(size(), rot).mul(this);
    }

    public VectorNf transform(VectorNf v) {
        return transform(v.toArray());
    }

    public VectorNf transform(float... vec) {
        final int size = size();
        if (size != vec.length) {
            throw new IllegalArgumentException("Matrix and vector sizes must be the same");
        }
        final VectorNf d = new VectorNf(size);
        for (int row = 0; row < size; row++) {
            float dot = 0;
            for (int col = 0; col < size; col++) {
                dot += mat[row][col] * vec[col];
            }
            d.set(row, dot);
        }
        return d;
    }

    @Override
    public MatrixNf floor() {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = GenericMath.floor(mat[row][col]);
            }
        }
        return d;
    }

    @Override
    public MatrixNf ceil() {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = (float) Math.ceil(mat[row][col]);
            }
        }
        return d;
    }

    @Override
    public MatrixNf round() {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = Math.round(mat[row][col]);
            }
        }
        return d;
    }

    @Override
    public MatrixNf abs() {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = Math.abs(mat[row][col]);
            }
        }
        return d;
    }

    @Override
    public MatrixNf negate() {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = -mat[row][col];
            }
        }
        return d;
    }

    @Override
    public MatrixNf transpose() {
        final int size = size();
        final MatrixNf d = new MatrixNf(size);
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                d.mat[row][col] = mat[col][row];
            }
        }
        return d;
    }

    @Override
    public float trace() {
        final int size = size();
        float trace = 0;
        for (int rowCol = 0; rowCol < size; rowCol++) {
            trace += mat[rowCol][rowCol];
        }
        return trace;
    }

    @Override
    public float determinant() {
        final int size = size();
        final float[][] m = deepClone(mat);
        float det;
        for (int i = 0; i < size - 1; i++) {
            for (int col = i + 1; col < size; col++) {
                det = m[i][i] < GenericMath.FLT_EPSILON ? 0 : m[i][col] / m[i][i];
                for (int row = i; row < size; row++) {
                    m[row][col] -= det * m[row][i];
                }
            }
        }
        det = 1;
        for (int i = 0; i < size; i++) {
            det *= m[i][i];
        }
        return det;
    }

    @Override
    public MatrixNf invert() {
        if (Math.abs(determinant()) < GenericMath.FLT_EPSILON) {
            throw new ArithmeticException("Cannot inverse a matrix with a zero determinant");
        }
        final int size = size();
        final AugmentedMatrixN augMat = new AugmentedMatrixN(this);
        final int augmentedSize = augMat.getAugmentedSize();
        for (int i = 0; i < size; i++) {
            for (int row = 0; row < size; row++) {
                if (i != row) {
                    final float ratio = augMat.get(row, i) / augMat.get(i, i);
                    for (int col = 0; col < augmentedSize; col++) {
                        augMat.set(row, col, augMat.get(row, col) - ratio * augMat.get(i, col));
                    }
                }
            }
        }
        for (int row = 0; row < size; row++) {
            final float div = augMat.get(row, row);
            for (int col = 0; col < augmentedSize; col++) {
                augMat.set(row, col, augMat.get(row, col) / div);
            }
        }
        return augMat.getAugmentation();
    }

    public Matrix2f toMatrix2() {
        return new Matrix2f(this);
    }

    public Matrix3f toMatrix3() {
        return new Matrix3f(this);
    }

    public Matrix4f toMatrix4() {
        return new Matrix4f(this);
    }

    public float[] toArray() {
        return toArray(false);
    }

    @Override
    public MatrixNf toFloat() {
        final int size = size();
        final float[] m = new float[size * size];
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                m[col + row * size] = (float) get(row, col);
            }
        }
        return new MatrixNf(m);
    }

    @Override
    public MatrixNd toDouble() {
        final int size = size();
        final double[] m = new double[size * size];
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                m[col + row * size] = (double) get(row, col);
            }
        }
        return new MatrixNd(m);
    }

    @Override
    public float[] toArray(boolean columnMajor) {
        final int size = size();
        final float[] array = new float[size * size];
        if (columnMajor) {
            for (int col = 0; col < size; col++) {
                for (int row = 0; row < size; row++) {
                    array[row + col * size] = mat[row][col];
                }
            }
        } else {
            for (int row = 0; row < size; row++) {
                System.arraycopy(mat[row], 0, array, row * size, size);
            }
        }
        return array;
    }

    @Override
    public String toString() {
        final int size = size();
        final StringBuilder builder = new StringBuilder();
        for (int row = 0; row < size; row++) {
            for (int col = 0; col < size; col++) {
                builder.append(mat[row][col]);
                if (col < size - 1) {
                    builder.append(' ');
                }
            }
            if (row < size - 1) {
                builder.append('\n');
            }
        }
        return builder.toString();
    }

    @Override
    public boolean equals(Object obj) {
        if (obj == this) {
            return true;
        }
        if (!(obj instanceof MatrixNf)) {
            return false;
        }
        return Arrays.deepEquals(mat, ((MatrixNf) obj).mat);
    }

    @Override
    public int hashCode() {
        return 79 * 5 + Arrays.deepHashCode(mat);
    }

    @Override
    public MatrixNf clone() {
        return new MatrixNf(this);
    }

    public static MatrixNf createScaling(VectorNf v) {
        return createScaling(v.toArray());
    }

    public static MatrixNf createScaling(float... vec) {
        final int size = vec.length;
        final MatrixNf m = new MatrixNf(size);
        for (int rowCol = 0; rowCol < size; rowCol++) {
            m.set(rowCol, rowCol, vec[rowCol]);
        }
        return m;
    }

    public static MatrixNf createTranslation(VectorNf v) {
        return createTranslation(v.toArray());
    }

    public static MatrixNf createTranslation(float... vec) {
        final int size = vec.length;
        final MatrixNf m = new MatrixNf(size + 1);
        for (int row = 0; row < size; row++) {
            m.set(row, size, vec[row]);
        }
        return m;
    }

    public static MatrixNf createRotation(int size, Complexf rot) {
        if (size < 2) {
            throw new IllegalArgumentException("Minimum matrix size is 2");
        }
        final MatrixNf m = new MatrixNf(size);
        rot = rot.normalize();
        m.set(0, 0, rot.getX());
        m.set(0, 1, -rot.getY());
        m.set(1, 0, rot.getY());
        m.set(1, 1, rot.getX());
        return m;
    }

    public static MatrixNf createRotation(int size, Quaternionf rot) {
        if (size < 3) {
            throw new IllegalArgumentException("Minimum matrix size is 3");
        }
        final MatrixNf m = new MatrixNf(size);
        rot = rot.normalize();
        m.set(0, 0, 1 - 2 * rot.getY() * rot.getY() - 2 * rot.getZ() * rot.getZ());
        m.set(0, 1, 2 * rot.getX() * rot.getY() - 2 * rot.getW() * rot.getZ());
        m.set(0, 2, 2 * rot.getX() * rot.getZ() + 2 * rot.getW() * rot.getY());
        m.set(1, 0, 2 * rot.getX() * rot.getY() + 2 * rot.getW() * rot.getZ());
        m.set(1, 1, 1 - 2 * rot.getX() * rot.getX() - 2 * rot.getZ() * rot.getZ());
        m.set(1, 2, 2 * rot.getY() * rot.getZ() - 2 * rot.getW() * rot.getX());
        m.set(2, 0, 2 * rot.getX() * rot.getZ() - 2 * rot.getW() * rot.getY());
        m.set(2, 1, 2 * rot.getY() * rot.getZ() + 2 * rot.getX() * rot.getW());
        m.set(2, 2, 1 - 2 * rot.getX() * rot.getX() - 2 * rot.getY() * rot.getY());
        return m;
    }

    /**
     * Creates a "look at" matrix for the given eye point.
     *
     * @param size The size of the matrix, minimum of 4
     * @param eye The position of the camera
     * @param at The point that the camera is looking at
     * @param up The "up" vector
     * @return A rotational transform that corresponds to a camera looking at the given point
     */
    public static MatrixNf createLookAt(int size, Vector3f eye, Vector3f at, Vector3f up) {
        if (size < 4) {
            throw new IllegalArgumentException("Minimum matrix size is 4");
        }
        final Vector3f f = at.sub(eye).normalize();
        up = up.normalize();
        final Vector3f s = f.cross(up).normalize();
        final Vector3f u = s.cross(f).normalize();
        final MatrixNf mat = new MatrixNf(size);
        mat.set(0, 0, s.getX());
        mat.set(0, 1, s.getY());
        mat.set(0, 2, s.getZ());
        mat.set(1, 0, u.getX());
        mat.set(1, 1, u.getY());
        mat.set(1, 2, u.getZ());
        mat.set(2, 0, -f.getX());
        mat.set(2, 1, -f.getY());
        mat.set(2, 2, -f.getZ());
        return mat.translate(eye.mul(-1).toVectorN());
    }

    /**
     * Creates a perspective projection matrix with the given (x) FOV, aspect, near and far planes
     *
     * @param size The size of the matrix, minimum of 4
     * @param fov The field of view in the x direction
     * @param aspect The aspect ratio, usually width/height
     * @param near The near plane, cannot be 0
     * @param far the far plane, zFar cannot equal zNear
     * @return A perspective projection matrix built from the given values
     */
    public static MatrixNf createPerspective(int size, double fov, double aspect, double near, double far) {
        return createPerspective(size, (float) fov, (float) aspect, (float) near, (float) far);
    }

    /**
     * Creates a perspective projection matrix with the given (x) FOV, aspect, near and far planes
     *
     * @param size The size of the matrix, minimum of 4
     * @param fov The field of view in the x direction
     * @param aspect The aspect ratio, usually width/height
     * @param near The near plane, cannot be 0
     * @param far the far plane, zFar cannot equal zNear
     * @return A perspective projection matrix built from the given values
     */
    public static MatrixNf createPerspective(int size, float fov, float aspect, float near, float far) {
        if (size < 4) {
            throw new IllegalArgumentException("Minimum matrix size is 4");
        }
        final MatrixNf perspective = new MatrixNf(size);
        final float scale = 1 / TrigMath.tan(fov * (float) TrigMath.HALF_DEG_TO_RAD);
        perspective.set(0, 0, scale / aspect);
        perspective.set(1, 1, scale);
        perspective.set(2, 2, (far + near) / (near - far));
        perspective.set(2, 3, 2 * far * near / (near - far));
        perspective.set(3, 2, -1);
        perspective.set(3, 3, 0);
        return perspective;
    }

    /**
     * Creates an orthographic viewing frustum built from the provided values
     *
     * @param size The size of the matrix, minimum of 4
     * @param right the right most plane of the viewing frustum
     * @param left the left most plane of the viewing frustum
     * @param top the top plane of the viewing frustum
     * @param bottom the bottom plane of the viewing frustum
     * @param near the near plane of the viewing frustum
     * @param far the far plane of the viewing frustum
     * @return A viewing frustum built from the provided values
     */
    public static MatrixNf createOrthographic(int size, double right, double left, double top, double bottom,
                                             double near, double far) {
        return createOrthographic(size, (float) right, (float) left, (float) top, (float) bottom, (float) near, (float) far);
    }

    /**
     * Creates an orthographic viewing frustum built from the provided values
     *
     * @param size The size of the matrix, minimum of 4
     * @param right the right most plane of the viewing frustum
     * @param left the left most plane of the viewing frustum
     * @param top the top plane of the viewing frustum
     * @param bottom the bottom plane of the viewing frustum
     * @param near the near plane of the viewing frustum
     * @param far the far plane of the viewing frustum
     * @return A viewing frustum built from the provided values
     */
    public static MatrixNf createOrthographic(int size, float right, float left, float top, float bottom,
                                             float near, float far) {
        if (size < 4) {
            throw new IllegalArgumentException("Minimum matrix size is 4");
        }
        final MatrixNf orthographic = new MatrixNf(size);
        orthographic.set(0, 0, 2 / (right - left));
        orthographic.set(1, 1, 2 / (top - bottom));
        orthographic.set(2, 2, -2 / (far - near));
        orthographic.set(0, 3, -(right + left) / (right - left));
        orthographic.set(1, 3, -(top + bottom) / (top - bottom));
        orthographic.set(2, 3, -(far + near) / (far - near));
        return orthographic;
    }

    private static float[][] deepClone(float[][] array) {
        final int size = array.length;
        final float[][] clone = array.clone();
        for (int i = 0; i < size; i++) {
            clone[i] = array[i].clone();
        }
        return clone;
    }

    private static class ImmutableIdentityMatrixN extends MatrixNf {
        public ImmutableIdentityMatrixN(int size) {
            super(size);
        }

        @Override
        public void set(int row, int col, float val) {
            throw new UnsupportedOperationException("You may not alter this matrix");
        }

        @Override
        public void setZero() {
            throw new UnsupportedOperationException("You may not alter this matrix");
        }
    }

    private static class AugmentedMatrixN {
        private final MatrixNf mat;
        private final MatrixNf aug;
        private final int size;

        private AugmentedMatrixN(MatrixNf mat) {
            this.mat = mat.clone();
            this.size = mat.size();
            aug = new MatrixNf(size);
        }

        private MatrixNf getAugmentation() {
            return aug;
        }

        private int getAugmentedSize() {
            return size * 2;
        }

        private float get(int row, int col) {
            if (col < size) {
                return mat.get(row, col);
            } else {
                return aug.get(row, col - size);
            }
        }

        private void set(int row, int col, float val) {
            if (col < size) {
                mat.set(row, col, val);
            } else {
                aug.set(row, col - size, val);
            }
        }
    }
}


package com.flowpowered.math.imaginary;

import java.io.Serializable;
import java.lang.Override;

import com.flowpowered.math.GenericMath;
import com.flowpowered.math.HashFunctions;
import com.flowpowered.math.TrigMath;
import com.flowpowered.math.vector.Vector2d;
import com.flowpowered.math.vector.Vector3d;

/**
 * Represent a complex number of the form x + yi. The x and y components are stored as doubles. This class is immutable.
 */
public class Complexd implements Imaginaryd, Comparable<Complexd>, Serializable, Cloneable {
    private static final long serialVersionUID = 1;
    /**
     * An immutable identity (0, 0) complex.
     */
    public static final Complexd ZERO = new Complexd(0, 0);
    /**
     * An immutable identity (1, 0) complex.
     */
    public static final Complexd IDENTITY = new Complexd(1, 0);
    private final double x;
    private final double y;
    private transient volatile boolean hashed = false;
    private transient volatile int hashCode = 0;

    /**
     * Constructs a new complex. The components are set to the identity (1, 0).
     */
    public Complexd() {
        this(1, 0);
    }

    /**
     * Constructs a new complex from the float components.
     *
     * @param x The x (real) component
     * @param y The y (imaginary) component
     */
    public Complexd(float x, float y) {
        this((double) x, (double) y);
    }

    /**
     * Constructs a new complex from the double components.
     *
     * @param x The x (real) component
     * @param y The y (imaginary) component
     */
    public Complexd(double x, double y) {
        this.x = x;
        this.y = y;
    }

    /**
     * Copy constructor.
     *
     * @param c The complex to copy
     */
    public Complexd(Complexd c) {
        this.x = c.x;
        this.y = c.y;
    }

    /**
     * Gets the x (real) component of this complex.
     *
     * @return The x (real) component
     */
    public double getX() {
        return x;
    }

    /**
     * Gets the y (imaginary) component of this complex.
     *
     * @return The y (imaginary) component
     */
    public double getY() {
        return y;
    }

    /**
     * Adds another complex to this one.
     *
     * @param c The complex to add
     * @return A new complex, which is the sum of both
     */
    public Complexd add(Complexd c) {
        return add(c.x, c.y);
    }

    /**
     * Adds the float components of another complex to this one.
     *
     * @param x The x (real) component of the complex to add
     * @param y The y (imaginary) component of the complex to add
     * @return A new complex, which is the sum of both
     */
    public Complexd add(float x, float y) {
        return add((double) x, (double) y);
    }

    /**
     * Adds the double components of another complex to this one.
     *
     * @param x The x (real) component of the complex to add
     * @param y The y (imaginary) component of the complex to add
     * @return A new complex, which is the sum of both
     */
    public Complexd add(double x, double y) {
        return new Complexd(this.x + x, this.y + y);
    }

    /**
     * Subtracts another complex from this one.
     *
     * @param c The complex to subtract
     * @return A new complex, which is the difference of both
     */
    public Complexd sub(Complexd c) {
        return sub(c.x, c.y);
    }

    /**
     * Subtracts the float components of another complex from this one.
     *
     * @param x The x (real) component of the complex to subtract
     * @param y The y (imaginary) component of the complex to subtract
     * @return A new complex, which is the difference of both
     */
    public Complexd sub(float x, float y) {
        return sub((double) x, (double) y);
    }

    /**
     * Subtracts the double components of another complex from this one.
     *
     * @param x The x (real) component of the complex to subtract
     * @param y The y (imaginary) component of the complex to subtract
     * @return A new complex, which is the difference of both
     */
    public Complexd sub(double x, double y) {
        return new Complexd(this.x - x, this.y - y);
    }

    /**
     * Multiplies the components of this complex by a float scalar.
     *
     * @param a The multiplication scalar
     * @return A new complex, which has each component multiplied by the scalar
     */
    public Complexd mul(float a) {
        return mul((double) a);
    }

    /**
     * Multiplies the components of this complex by a double scalar.
     *
     * @param a The multiplication scalar
     * @return A new complex, which has each component multiplied by the scalar
     */
    @Override
    public Complexd mul(double a) {
        return new Complexd(x * a, y * a);
    }

    /**
     * Multiplies another complex with this one.
     *
     * @param c The complex to multiply with
     * @return A new complex, which is the product of both
     */
    public Complexd mul(Complexd c) {
        return mul(c.x, c.y);
    }

    /**
     * Multiplies the float components of another complex with this one.
     *
     * @param x The x (real) component of the complex to multiply with
     * @param y The y (imaginary) component of the complex to multiply with
     * @return A new complex, which is the product of both
     */
    public Complexd mul(float x, float y) {
        return mul((double) x, (double) y);
    }

    /**
     * Multiplies the double components of another complex with this one.
     *
     * @param x The x (real) component of the complex to multiply with
     * @param y The y (imaginary) component of the complex to multiply with
     * @return A new complex, which is the product of both
     */
    public Complexd mul(double x, double y) {
        return new Complexd(
                this.x * x - this.y * y,
                this.x * y + this.y * x);
    }

    /**
     * Divides the components of this complex by a float scalar.
     *
     * @param a The division scalar
     * @return A new complex, which has each component divided by the scalar
     */
    public Complexd div(float a) {
        return div((double) a);
    }

    /**
     * Divides the components of this complex by a double scalar.
     *
     * @param a The division scalar
     * @return A new complex, which has each component divided by the scalar
     */
    @Override
    public Complexd div(double a) {
        return new Complexd(x / a, y / a);
    }

    /**
     * Divides this complex by another one.
     *
     * @param c The complex to divide with
     * @return The quotient of the two complexes
     */
    public Complexd div(Complexd c) {
        return div(c.x, c.y);
    }

    /**
     * Divides this complex by the float components of another one.
     *
     * @param x The x (real) component of the complex to divide with
     * @param y The y (imaginary) component of the complex to divide with
     * @return The quotient of the two complexes
     */
    public Complexd div(float x, float y) {
        return div((double) x, (double) y);
    }

    /**
     * Divides this complex by the double components of another one.
     *
     * @param x The x (real) component of the complex to divide with
     * @param y The y (imaginary) component of the complex to divide with
     * @return The quotient of the two complexes
     */
    public Complexd div(double x, double y) {
        final double d = x * x + y * y;
        return new Complexd(
                (this.x * x + this.y * y) / d,
                (this.y * x - this.x * y) / d);
    }

    /**
     * Returns the dot product of this complex with another one.
     *
     * @param c The complex to calculate the dot product with
     * @return The dot product of the two complexes
     */
    public double dot(Complexd c) {
        return dot(c.x, c.y);
    }

    /**
     * Returns the dot product of this complex with the float components of another one.
     *
     * @param x The x (real) component of the complex to calculate the dot product with
     * @param y The y (imaginary) component of the complex to calculate the dot product with
     * @return The dot product of the two complexes
     */
    public double dot(float x, float y) {
        return dot((double) x, (double) y);
    }

    /**
     * Returns the dot product of this complex with the double components of another one.
     *
     * @param x The x (real) component of the complex to calculate the dot product with
     * @param y The y (imaginary) component of the complex to calculate the dot product with
     * @return The dot product of the two complexes
     */
    public double dot(double x, double y) {
        return this.x * x + this.y * y;
    }

    /**
     * Rotates a vector by this complex.
     *
     * @param v The vector to rotate
     * @return The rotated vector
     */
    public Vector2d rotate(Vector2d v) {
        return rotate(v.getX(), v.getY());
    }

    /**
     * Rotates the float components of a vector by this complex.
     *
     * @param x The x component of the vector
     * @param y The y component of the vector
     * @return The rotated vector
     */
    public Vector2d rotate(float x, float y) {
        return rotate((double) x, (double) y);
    }

    /**
     * Rotates the double components of a vector by this complex.
     *
     * @param x The x component of the vector
     * @param y The y component of the vector
     * @return The rotated vector
     */
    public Vector2d rotate(double x, double y) {
        final double length = length();
        if (Math.abs(length) < GenericMath.DBL_EPSILON) {
            throw new ArithmeticException("Cannot rotate by the zero complex");
        }
        final double nx = this.x / length;
        final double ny = this.y / length;
        return new Vector2d(x * nx - y * ny, y * nx + x * ny);
    }

    /**
     * Returns a unit vector pointing in the same direction as this complex on the complex plane.
     *
     * @return The vector representing the direction this complex is pointing to
     */
    public Vector2d getDirection() {
        return new Vector2d(x, y).normalize();
    }

    /**
     * Returns the angle in radians formed by the direction vector of this complex on the complex plane.
     *
     * @return The angle in radians of the direction vector of this complex
     */
    public double getAngleRad() {
        return (double) TrigMath.atan2(y, x);
    }

    /**
     * Returns the angle in degrees formed by the direction vector of this complex on the complex plane.
     *
     * @return The angle in degrees of the direction vector of this complex
     */
    public double getAngleDeg() {
        return (double) Math.toDegrees(getAngleRad());
    }

    /**
     * Returns the conjugate of this complex.
 Conjugation of a complex a is an operation returning complex a' such that a' * a = a * a' = |a|2 where
     * |a|2 is squared length of a.
     *
     * @return A new complex, which is the conjugate of this one
     */
    @Override
    public Complexd conjugate() {
        return new Complexd(x, -y);
    }

    /**
     * Returns the inverse of this complex.
 Inversion of a complex a returns complex a-1 = a' / |a|2 where a' is {@link #conjugate()
     * conjugation} of a, and |a|2 is squared length of a.
 For any complexes a, b, c, such that a * b = c equations
     * a-1 * c = b and c * b-1 = a are true.
     *
     * @return A new complex, which is the inverse of this one
     */
    @Override
    public Complexd invert() {
        final double lengthSquared = lengthSquared();
        if (Math.abs(lengthSquared) < GenericMath.DBL_EPSILON) {
            throw new ArithmeticException("Cannot invert a complex of length zero");
        }
        return conjugate().div(lengthSquared);
    }

    /**
     * Returns the square of the length of this complex.
     *
     * @return The square of the length
     */
    @Override
    public double lengthSquared() {
        return x * x + y * y;
    }

    /**
     * Returns the length of this complex.
     *
     * @return The length
     */
    @Override
    public double length() {
        return (double) Math.sqrt(lengthSquared());
    }

    /**
     * Normalizes this complex.
     *
     * @return A new complex of unit length
     */
    @Override
    public Complexd normalize() {
        final double length = length();
        if (Math.abs(length) < GenericMath.DBL_EPSILON) {
            throw new ArithmeticException("Cannot normalize the zero complex");
        }
        return new Complexd(x / length, y / length);
    }

    /**
     * Converts this complex to a quaternion by
     * using (0, 0, 1) as a rotation axis.
     *
     * @return A quaternion of this rotation around the unit z
     */
    public Quaterniond toQuaternion() {
        return toQuaternion(Vector3d.UNIT_Z);
    }

    /**
     * Converts this complex to a quaternion by
     * using the provided vector as a rotation axis.
     *
     * @param axis The rotation axis
     * @return A quaternion of this rotation around the given axis
     */
    public Quaterniond toQuaternion(Vector3d axis) {
        return toQuaternion(axis.getX(), axis.getY(), axis.getZ());
    }

    /**
     * Converts this complex to a quaternion by
     * using the provided float components vector
     * as a rotation axis.
     *
     * @param x The x component of the axis vector
     * @param y The y component of the axis vector
     * @param z The z component of the axis vector
     * @return A quaternion of this rotation around the given axis
     */
    public Quaterniond toQuaternion(float x, float y, float z) {
        return toQuaternion((double) x, (double) y, (double) z);
    }

    /**
     * Converts this complex to a quaternion by
     * using the provided double components vector
     * as a rotation axis.
     *
     * @param x The x component of the axis vector
     * @param y The y component of the axis vector
     * @param z The z component of the axis vector
     * @return A quaternion of this rotation around the given axis
     */
    public Quaterniond toQuaternion(double x, double y, double z) {
        return Quaterniond.fromAngleRadAxis(getAngleRad(), x, y, z);
    }

    @Override
    public Complexf toFloat() {
        return new Complexf(x, y);
    }

    @Override
    public Complexd toDouble() {
        return new Complexd(x, y);
    }

    @Override
    public boolean equals(Object o) {
        if (this == o) {
            return true;
        }
        if (!(o instanceof Complexd)) {
            return false;
        }
        final Complexd complex = (Complexd) o;
        if (Double.compare(complex.x, x) != 0) {
            return false;
        }
        if (Double.compare(complex.y, y) != 0) {
            return false;
        }
        return true;
    }

    @Override
    public int hashCode() {
        if (!hashed) {
            final int result = (x != +0.0f ? HashFunctions.hash(x) : 0);
            hashCode = 31 * result + (y != +0.0f ? HashFunctions.hash(y) : 0);
            hashed = true;
        }
        return hashCode;
    }

    @Override
    public int compareTo(Complexd c) {
        return (int) Math.signum(lengthSquared() - c.lengthSquared());
    }

    @Override
    public Complexd clone() {
        return new Complexd(this);
    }

    @Override
    public String toString() {
        return "(" + x + ", " + y + ")";
    }

    /**
     * Creates a new complex from the double real component.
     *
     *
The {@link #ZERO} constant is re-used when {@code x} is 0.

     *
     * @param x The x (real) component
     * @return The complex created from the double real component
     */
    public static Complexd fromReal(double x) {
         return x == 0 ? ZERO : new Complexd(x, 0);
    }

    /**
     * Creates a new complex from the double imaginary components.
     *
     *
The {@link #ZERO} constant is re-used when {@code y} is 0.

     *
     * @param y The y (imaginary) component
     * @return The complex created from the double imaginary component
     */
    public static Complexd fromImaginary(double y) {
         return y == 0 ? ZERO : new Complexd(0, y);
    }

    /**
     * Creates a new complex from the double components.
     *
     *
The {@link #ZERO} constant is re-used when both {@code x} and {@code z} are 0.

     *
     * @param x The x (real) component
     * @param y The y (imaginary) component
     * @return The complex created from the double components
     */
    public static Complexd from(double x, double y) {
         return x == 0 && y == 0 ? ZERO : new Complexd(x, y);
    }

    /**
     * Creates a new complex from the angle defined from the first to the second vector.
     *
     * @param from The first vector
     * @param to The second vector
     * @return The complex defined by the angle between the vectors
     */
    public static Complexd fromRotationTo(Vector2d from, Vector2d to) {
        return fromAngleRad(TrigMath.acos(from.dot(to) / (from.length() * to.length())));
    }

    /**
     * Creates a new complex from the angle defined from the first to the second vector.
     *
     * @param from The first vector
     * @param to The second vector
     * @return The complex defined by the angle between the vectors
     */
    public static Complexd fromRotationTo(Vector3d from, Vector3d to) {
        return fromAngleRad(TrigMath.acos(from.dot(to) / (from.length() * to.length())));
    }

    /**
     * Creates a new complex from the float angle in degrees.
     *
     * @param angle The angle in degrees
     * @return The complex defined by the angle
     */
    public static Complexd fromAngleDeg(float angle) {
        return fromAngleRad(Math.toRadians(angle));
    }

    /**
     * Creates a new complex from the float angle in radians.
     *
     * @param angle The angle in radians
     * @return The complex defined by the angle
     */
    public static Complexd fromAngleRad(float angle) {
        return fromAngleRad((double) angle);
    }

    /**
     * Creates a new complex from the double angle in radians.
     *
     * @param angle The angle in radians
     * @return The complex defined by the angle
     */
    public static Complexd fromAngleDeg(double angle) {
        return fromAngleRad((double) Math.toRadians(angle));
    }

    /**
     * Creates a new complex from the double angle in radians.
     *
     * @param angle The angle in radians
     * @return The complex defined by the angle
     */
    public static Complexd fromAngleRad(double angle) {
        return new Complexd(TrigMath.cos(angle), TrigMath.sin(angle));
    }
}