import javax.vecmath.*;
import javax.media.j3d.*;
import java.util.*;
import java.io.*;

/**
 *
 * @author Bobby Martin
 * @copyright 2000
 * @version 0.1
 * @since 0.1
 */
public class MatrixUtil
{
    /**
     * Retrieves the euler angles from the matrix.  If you
     * setEuler on a Transform3D, retrieve the matrix3d from
     * the Transform3D, and get the euler angles back, you
     * will get these same angles, normalized to range from
     * 0, 2*Math.PI
     * @param matrix the matrix from which to retrieve euler angles
     * @param eulers vector3d in which the euler angles will be put
     */
    public static void getEuler(Matrix3d matrix,
                                Vector3d euler1, Vector3d euler2)
    {
        double[] mat = new double[9];
        for( int i = 0; i < 9; ++i )
        {
            mat[i] = matrix.getElement(i/3,i%3);
        }
        double[] sins = new double[3];
        double[] coss = new double[3];

        final double accuracy = 0.00001;
        if( MathUtil.snap(mat[2],accuracy) != 0.0 )
        {
            coss[1] = Math.pow((mat[7] * mat[3] +
                                mat[8] * (-mat[6]) * mat[0])/mat[2],
                               1/2.0);
        }
        else if( MathUtil.snap(mat[1],accuracy) != 0.0 )
        {
            coss[1] = Math.pow((-(mat[8] * mat[3]) +
                                mat[7] * (-mat[6]) * mat[0])/mat[1],
                               1/2.0);
        }
        else if( MathUtil.snap(mat[5],accuracy) != 0.0 )
        {
            coss[1] = Math.pow((-(mat[7] * mat[0]) +
                                mat[8] * (-mat[6]) * mat[3])/mat[5],
                               1/2.0);
        }
        else if( MathUtil.snap(mat[4],accuracy) != 0.0 )
        {
            coss[1] = Math.pow((mat[8] * mat[0] +
                                mat[7] * (-mat[6]) * mat[3])/mat[4],
                               1/2.0);
        }
        else
        {
            System.err.println("No suitable d4");
        }

        eulerHelper(sins, coss, mat, euler1, matrix);
        coss[1] = -coss[1];
        eulerHelper(sins, coss, mat, euler2, matrix);
    }

    /**
     * Calculates the angles given the first cos value.
     */
    protected static void eulerHelper(double[] sins, double[] coss,
                                      double[] mat, Vector3d eulers,
                                      Matrix3d matrix)
    {
        sins[0] = mat[7]/coss[1];
        coss[0] = mat[8]/coss[1];
        coss[2] = mat[0]/coss[1];
        sins[2] = mat[3]/coss[1];
        sins[1] = -mat[6];

        double[] angles = new double[3];
        for( int i = 0; i < 3; ++i )
        {
            if( coss[i] < -1 || coss[i] > 1 )
            {
                coss[i] = MathUtil.snap(coss[i], 0.001);
            }

            //Cos angle ranges 0, PI
            angles[i] = Math.acos(coss[i]);
            if( sins[i] < 0 )
            {
                angles[i] = 2*Math.PI - angles[i];
            }
            if( DEBUG)
            {
                if( MathUtil.snapTo(Math.sin(angles[i]), 0.0001, sins[i])
                    != sins[i] )
                {
                    System.out.println("Angle is " + angles[i]);
                    System.out.println("Matrix is " + matrix);
                    System.out.println("My sin: " + Math.sin(angles[i]) +
                                       " real sin: " + sins[i]);
                }
                if( MathUtil.snapTo(Math.cos(angles[i]), 0.0001, coss[i])
                    != coss[i] )
                {
                    System.out.println("Angle is " + angles[i]);
                    System.out.println("Matrix is " + matrix);
                    System.out.println("My cos: " + Math.cos(angles[i]) +
                                       " real cos: " + coss[i]);
                }
            } //end debug code
        }

        eulers.set(angles);
    }

    /**
     * Retrieves the euler angles from the matrix.  If you
     * setEuler on a Transform3D, retrieve the matrix3d from
     * the Transform3D, and get the euler angles back, you
     * will get these same angles, normalized to range from
     * 0, 2*Math.PI
     * @param matrix the matrix from which to retrieve euler angles
     * @return vector3d containing the euler angles
     */
    public static Vector3d getEuler(Matrix3d matrix)
    {
        Vector3d retval = new Vector3d();
        Vector3d dummy = new Vector3d();
        getEuler(matrix,retval, dummy);
        return retval;
    }

    /**
     * Retrieves the euler angles from the matrix.  If you
     * setEuler on a Transform3D, retrieve the matrix3d from
     * the Transform3D, and get the euler angles back, you
     * will get these same angles, normalized to range from
     * 0, 2*Math.PI
     * The euler angle returned is the one closest to the one you pass
     * in, modulus 2*Math.PI.
     * @param matrix the matrix from which to retrieve euler angles
     * @return vector3d containing the euler angles
     */
    public static Vector3d getClosestEuler(Matrix3d matrix,
                                           Vector3d closeAngle)
    {
        Vector3d euler1 = new Vector3d();
        Vector3d euler2 = new Vector3d();
        getEuler(matrix, euler1, euler2);

        if( angleDistance(closeAngle, euler1) <
            angleDistance(closeAngle, euler2) )
        {
            return euler1;
        }
        else
        {
            return euler2;
        }
    }

    /**
     * Retrieves the euler angles from the matrix.  If you
     * setEuler on a Transform3D, retrieve the matrix3d from
     * the Transform3D, and get the euler angles back, you
     * will get these same angles, normalized to range from
     * 0, 2*Math.PI
     * The euler angle returned is the one closest to the one you pass
     * in, modulus 2*Math.PI.
     * @param matrix the matrix from which to retrieve euler angles
     * @return vector3d containing the euler angles
     */
    public static void getClosestEuler(Matrix3d matrix,
                                       Vector3d closeAngle,
                                       Vector3d retval)
    {
        Vector3d euler1 = new Vector3d();
        getEuler(matrix, euler1, retval);

        System.err.println("Euler 1: " + euler1);
        System.err.println("Euler 2: " + retval);
        System.err.println("Test euler: " + closeAngle);
        System.err.println("Distance 1: " +
                               angleDistance(closeAngle, euler1));
        System.err.println("Distance 2: " +
                               angleDistance(closeAngle, retval));

        if( angleDistance(closeAngle, euler1) <
            angleDistance(closeAngle, retval) )
        {
            retval.set(euler1);
        }
        System.err.println("Closest euler: " + retval);
    }

    public static double angleDistance(Vector3d angle1, Vector3d angle2)
    {
        Vector3d delta = new Vector3d(angle1);
        delta.sub(angle2);
        final double twopi = 2 * Math.PI;

        System.err.println("Unmodded delta: " + delta);
        delta.x = MathUtil.mod(delta.x, twopi);
        delta.y = MathUtil.mod(delta.y, twopi);
        delta.z = MathUtil.mod(delta.z, twopi);

        if( delta.x > Math.PI ) delta.x = twopi - delta.x;
        if( delta.y > Math.PI ) delta.y = twopi - delta.y;
        if( delta.z > Math.PI ) delta.z = twopi - delta.z;

        System.err.println("Modded delta: " + delta);
        return delta.lengthSquared();
    }

    public static void setEuler(Matrix3d matrix, Vector3d euler)
    {
        double d = Math.sin(euler.x);
        double d1 = Math.sin(euler.y);
        double d2 = Math.sin(euler.z);
        double d3 = Math.cos(euler.x);
        double d4 = Math.cos(euler.y);
        double d5 = Math.cos(euler.z);
        matrix.setElement(0,0,d4 * d5);
        matrix.setElement(0,1,-(d3 * d2) + d * d1 * d5);
        matrix.setElement(0,2,d * d2 + d3 * d1 * d5);
        matrix.setElement(1,0,d4 * d2);
        matrix.setElement(1,1,d3 * d5 + d * d1 * d2);
        matrix.setElement(1,2,-(d * d5) + d3 * d1 * d2);
        matrix.setElement(2,0,-d1);
        matrix.setElement(2,1,d * d4);
        matrix.setElement(2,2,d3 * d4);
    }

    //test code follows
    public static final boolean DEBUG = false;

    public static void debug(String s)
    {
        if( DEBUG )
        {
            System.err.println(s);
        }
    }

    public static void main(String[] args)
    {
        double fifteen = Math.PI/12;
        double[][] eulers = new double[24*24*24][3];
        for( int i = 0; i < 24; ++i )
            for( int j = 0; j < 24; ++j )
                for( int k = 0; k < 24; ++k )
                {
                    eulers[24*24*i+24*j+k][0] = i*24;
                    eulers[24*24*i+24*j+k][1] = j*24;
                    eulers[24*24*i+24*j+k][2] = k*24;
                }

        System.out.println("Made eulers");
        Vector3d euler = new Vector3d();
        Matrix3d matrix = new Matrix3d();
        Matrix3d mymatrix = new Matrix3d();
        Vector3d myeuler = new Vector3d();
        Transform3D xform = new Transform3D();
        for( int i = 0; i < eulers.length; ++i )
        {
            String output = "";
            euler.set(eulers[i][0], eulers[i][1], eulers[i][2]);

            output += "Original euler: " + euler + "\n";
            MatrixUtil.setEuler(matrix,euler);
            output += "Matrix after their euler:" + "\n";
            output += matrix + "\n";
            myeuler = MatrixUtil.getEuler(matrix);
            output += "My euler: " + myeuler + "\n";
            MatrixUtil.setEuler(mymatrix,myeuler);
            output += "Matrix after my euler:" + "\n";
            output += mymatrix + "\n";

            if( !mymatrix.epsilonEquals(matrix,0.001) )
            {
                System.out.println(output);
            }
            xform.setIdentity();
            xform.setEuler(euler);
            xform.get(matrix);
            mymatrix.setIdentity();
            MatrixUtil.setEuler(mymatrix, euler);
            if( !mymatrix.epsilonEquals(matrix,0.001) )
            {
                System.out.println("Error in MatrixUtil.setEuler");
            }
        }
    }
}