package cz.fidentis.analyst.icp;

import javax.vecmath.Matrix4d;
import javax.vecmath.Quat4d;

/**
 * This class represents quaternions. 
 * Primary, it fixes the error (?) in the {@code Quat4d} constructor, which normalizes
 * the given coordinates for some reason. Moreover, this extension
 * introduces useful methods that accelerate the ICP calculation a bit.
 *
 * @author Maria Kocurekova
 */
public final class Quaternion extends Quat4d {
    
    /**
     * Builds a quaternion from its components.
     *
     * @param w Scalar component.
     * @param x First vector component.
     * @param y Second vector component.
     * @param z Third vector component.
     */
    public Quaternion(double x, double y, double z, double w) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }
    
    /**
     * Builds a quaternion from eigenvalue decomposition.
     * 
     * @param ev eigenvalue decomposition.
     */
    public Quaternion(EigenvalueDecomposition ev) {
        Matrix4d eigD = ev.getD();
        Matrix4d eigM = ev.getV();
        int max = 0;

        //Finding quaternion q representing the rotation
        // (it is the eigenvector corresponding to the largest eigenvalue of sumMatrix)
        for (int i = 0; i < 4; i++) {
            if (eigD.getElement(max, max) <= eigD.getElement(i, i)) {
                max = i;
            }
        }
        
        this.x = eigM.getElement(1, max);
        this.y = eigM.getElement(2, max);
        this.z = eigM.getElement(3, max);
        this.w = eigM.getElement(0, max);
    }

    /**
     * Returns the conjugate quaternion of the instance.
     * It method is equivalent to calling the {@link #conjugate} method, 
     * but it is more effective and readable when the origin quaternion has to be preserved.
     *
     * @return the conjugate quaternion
     */
    public Quaternion getConjugate() {
        return new Quaternion(-x, -y, -z, w);
    }

    /**
     * Returns the Hamilton product of two quaternions.
     * It is equivalent to {@code q1.mul(q2)} of {@code Quat4d}.
     * But calling this method it is faster and more readable if the first quaternion 
     * has to be preserved (and then cloned first).
     *
     * @param q1 First quaternion.
     * @param q2 Second quaternion.
     * @return the Hamilton product of two quaternions
     */
    public static Quaternion multiply(final Quaternion q1, final Quaternion q2) {
        final double w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
        final double x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
        final double y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
        final double z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
        return new Quaternion(x, y, z, w);
    }

    /**
     * Convert a Quaternion to a rotation matrix. 
     * It is equivalent to but bit faster than calling 
     * {@code new Matrix4d(quternion, new Vector3d(0,0,0), 1);}
     *
     * @return a rotation matrix (4x4)
     */
    public Matrix4d toMatrix(){
        double xx = x * x;
        double xy = x * y;
        double xz = x * z;
        double xw = x * w;
        double yy = y * y;
        double yz = y * z;
        double yw = y * w;
        double zz = z * z;
        double zw = z * w;

        return new Matrix4d(
                1 - 2 * ( yy + zz ), 2 * ( xy + zw),      2 * ( xz - yw),      0,
                2 * ( xy - zw ),     1 - 2 * ( xx + zz ), 2 * ( yz + xw ),     0,
                2 * ( xz + yw ),     2 * ( yz - xw ),     1 - 2 * ( xx + yy ), 0,
                0,                   0,                   0,                   1
        );
    }

}
