/*
 * Copyright (C) 2008 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package android.hardware.cts;

import junit.framework.Assert;

import android.content.Context;
import android.hardware.SensorManager;
import android.os.PowerManager;

import java.util.Random;

public class SensorManagerStaticTest extends SensorTestCase {
    private static final String TAG = "SensorManagerTest";

    // local float version of PI
    private static final float FLOAT_PI = (float) Math.PI;


    private PowerManager.WakeLock mWakeLock;

    @Override
    protected void setUp() throws Exception {
        Context context = getContext();
        PowerManager pm = (PowerManager) context.getSystemService(Context.POWER_SERVICE);
        mWakeLock = pm.newWakeLock(PowerManager.PARTIAL_WAKE_LOCK, TAG);

        mWakeLock.acquire();
    }

    @Override
    protected void tearDown(){
        if (mWakeLock != null && mWakeLock.isHeld()) {
            mWakeLock.release();
        }
    }

    // SensorManager Tests
    public void testGetAltitude() throws Exception {
        float r, q;
        float altitude;

        // identity property
        for (r = 0.5f; r < 1.3f; r += 0.1f) {

            altitude = SensorManager.getAltitude(r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE,
                                                 r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE);
            assertRoughlyEqual("getAltitude identity property violated.", altitude, 0.0f, 0.1f);
        }

        // uniform increasing as pressure decreases property
        float prevAltitude = 1e5f; // 100km ceiling
        for (r = 0.5f; r < 1.3f; r += 0.01f) {
            altitude = SensorManager.getAltitude(SensorManager.PRESSURE_STANDARD_ATMOSPHERE,
                                                 r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE);

            assertTrue("getAltitude result has to decrease as p increase.", prevAltitude > altitude);
            prevAltitude = altitude;
        }

        // compare to a reference algorithm
        final float coef = 1.0f / 5.255f;
        for (r = 0.8f; r < 1.3f; r += 0.1f) {
            for (q = 1.1f * r; q > 0.5f * r; q -= 0.1f * r) {
                float p0 = r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE;
                float p  = q * SensorManager.PRESSURE_STANDARD_ATMOSPHERE;

                float t1 = SensorManager.getAltitude(p0, p);
                float t2 = 44330.f*(1.0f- (float) Math.pow(p/p0, coef));

                assertRoughlyEqual(
                      String.format("getAltitude comparing to reference algorithm failed. " +
                          "Detail: getAltitude(%f, %f) => %f, reference => %f",
                          p0, p, t1, t2),
                      t1, t2, 100.f);
            }
        }

    }

    public void testGetAngleChange() throws Exception {
        TestDataGenerator data = new TestDataGenerator();

        int i;
        float [] rotv = new float[3];
        float [] rotv2 = new float[3];

        // test many instances
        for (i=0; i<100; ++i) {
            float [] R1, R12, R2;
            // azimuth(yaw) pitch roll
            data.nextRotationAngles(rotv);
            R1 = mat9VRot(rotv); // random base

            // azimuth(yaw) pitch roll
            data.nextRotationAngles(rotv);
            R12 = mat9VRot(rotv);
            R2 = mat9Mul(R1, R12); // apply another random rotation

            // test different variations of input matrix format
            switch(i & 3) {
                case 0:
                    SensorManager.getAngleChange(rotv2, R2, R1);
                    break;
                case 1:
                    SensorManager.getAngleChange(rotv2, mat9to16(R2), R1);
                    break;
                case 2:
                    SensorManager.getAngleChange(rotv2, R2, mat9to16(R1));
                    break;
                case 3:
                    SensorManager.getAngleChange(rotv2, mat9to16(R2), mat9to16(R1));
                    break;
            }

            // check range
            assertRotationAnglesValid("getAngleChange result out of range.", rotv2);

            // avoid directly checking the rotation angles to avoid corner cases
            float [] R12rt = mat9T(mat9VRot(rotv2));
            float [] RI = mat9Mul(R12rt, R12);

            assertRoughlyEqual(
                String.format("getAngleChange result is incorrect. Details: case %d, " +
                    "truth = [%f, %f, %f], result = [%f, %f, %f]", i, rotv[0], rotv[1], rotv[2],
                    rotv2[0], rotv2[1], rotv2[2]),
                RI[0] + RI[4] + RI[8], 3.f, 1e-4f);
        }
    }

    public void testGetInclination() throws Exception {
        TestDataGenerator data = new TestDataGenerator();

        int i;
        float [] rotv = new float[3];
        float [] rotv2 = new float[3];
        float [] rotv3;

        // test many instances
        for (i = 0; i < 100; ++i) {
            float [] R;
            float angle;
            angle = (data.nextFloat()-0.5f) * FLOAT_PI;
            R = mat9Rot(SensorManager.AXIS_X, -angle);

            float angler = ((i&1) != 0) ?
                    SensorManager.getInclination(mat9to16(R)) : SensorManager.getInclination(R);
            assertRoughlyEqual(
                String.format(
                    "getInclination return incorrect result. Detail: case %d, truth %f, result %f.",
                    i, angle, angler),
                angle, angler, 1e-4f);
        }
    }

    public void testGetOrientation() throws Exception {
        TestDataGenerator data = new TestDataGenerator();

        int i;
        float [] rotv = new float[3];
        float [] rotv2 = new float[3];
        float [] rotv3;

        // test many instances
        for (i=0; i<100; ++i) {
            float [] R;
            // yaw pitch roll
            data.nextRotationAngles(rotv);
            R = mat9VRot(rotv);

            rotv3 = SensorManager.getOrientation( ((i&1) != 0) ? R : mat9to16(R), rotv2);
            assertTrue("getOrientaion has to return the array passed in argument", rotv3 == rotv2);

            // check range
            assertRotationAnglesValid("getOrientation result out of range.", rotv2);

            // Avoid directly comparing rotation angles. Instead, compare the rotation matrix.
            float [] Rr = mat9T(mat9VRot(rotv2));
            float [] RI = mat9Mul(Rr, R);

            assertRoughlyEqual(
                String.format("getOrientation result is incorrect. Details: case %d, " +
                    "truth = [%f, %f, %f], result = [%f, %f, %f]", i, rotv[0], rotv[1], rotv[2],
                    rotv2[0], rotv2[1], rotv2[2]),
                RI[0] + RI[4] + RI[8], 3.f, 1e-4f);
        }
    }

    public void testGetQuaternionFromVector() throws Exception {
        TestDataGenerator data = new TestDataGenerator();

        int i;
        float [] v;
        float [] q = new float[4];
        float [] q2 = new float[4];
        float [] v3 = new float[3];
        float [] v4 = new float[4];
        float [] v5 = new float[5];
        float [][] vs = new float[][] {v3, v4, v5};

        float [] xyzth = new float[4];
        for (i = 0; i < 100; ++i) {
            float c, s;

            data.nextRotationAxisAngle(xyzth);

            c = (float) Math.cos(xyzth[3]);
            s = (float) Math.sin(xyzth[3]);
            if (c < 0.f) {
                c = -c;
                s = -s;
            }

            v = vs[i%3];
            switch(i%3) {
                case 2:
                    v[4] = data.nextBoolean() ? data.nextFloat() : -1.f;
                case 1:
                    v[3] = c;
                case 0:
                    v[0] = s * xyzth[0];
                    v[1] = s * xyzth[1];
                    v[2] = s * xyzth[2];
            }

            q2[0] = c;
            q2[1] = v[0];
            q2[2] = v[1];
            q2[3] = v[2];

            SensorManager.getQuaternionFromVector(q, v);
            assertVectorRoughlyEqual(
                String.format("getQuaternionFromVector returns wrong results, Details: case %d, " +
                    "truth = (%f, %f, %f, %f), result = (%f, %f, %f, %f).",
                    i, q2[0], q2[1], q2[2], q2[3], q[0], q[1], q[2], q[3]),
                q, q2, 1e-4f);
        }
    }

    public void testGetRotationMatrix() throws Exception {
        TestDataGenerator data = new TestDataGenerator();
        final float gravity = 9.81f;
        final float magStrength = 50.f;

        int i;
        float [] gm = new float[9];
        float [] rotv = new float[3];
        float [] gI = null;
        float [] mI = null;
        float [] Rr = new float[9];
        float [] Ir = new float[9];

        gm[6] = gravity; // m/s^2, first column gravity

        // test many instances
        for (i=0; i<100; ++i) {
            float [] Rt;
            float incline;
            // yaw pitch roll
            data.nextRotationAngles(rotv);
            Rt = mat9T(mat9VRot(rotv)); // from world frame to phone frame
            //Rt = mat9I();

            incline = -0.9f * (data.nextFloat() - 0.5f) * FLOAT_PI; // ~ +-80 degrees
            //incline = 0.f;
            gm[4] = magStrength * (float) Math.cos(-incline); // positive means rotate downwards
            gm[7] = magStrength * (float) Math.sin(-incline);

            float [] gmb = mat9Mul(Rt, gm); // do not care about right most column
            gI = mat9Axis(gmb, SensorManager.AXIS_X);
            mI = mat9Axis(gmb, SensorManager.AXIS_Y);

            assertTrue("getRotationMatrix returns false on valid inputs",
                SensorManager.getRotationMatrix(Rr, Ir, gI, mI));

            float [] n = mat9Mul(Rr, Rt);
            assertRoughlyEqual(
                String.format("getRotationMatrix returns incorrect R matrix. " +
                    "Details: case %d, truth R = %s, result R = %s.",
                    i, mat9ToStr(mat9T(Rt)), mat9ToStr(Rr)),
                n[0] + n[4] + n[8], 3.f, 1e-4f);


            // Magnetic incline is defined so that it means the magnetic field lines is formed
            // by rotate local y axis around -x axis by incline angle. However, I matrix is
            // defined as (according to document):
            //     [0 m 0] = I * R * geomagnetic,
            // which means,
            //     I' * [0 m 0] = R * geomagnetic.
            // Thus, I' = Rot(-x, incline) and I = Rot(-x, incline)' = Rot(x, incline)
            float [] Ix = mat9Rot(SensorManager.AXIS_X, incline);
            assertVectorRoughlyEqual(
                String.format("getRotationMatrix returns incorrect I matrix. " +
                    "Details: case %d, truth I = %s, result I = %s.",
                    i, mat9ToStr(Ix), mat9ToStr(Ir)),
                Ix, Ir, 1e-4f);
        }

        // test 16 element inputs
        float [] Rr2 = new float[16];
        float [] Ir2 = new float[16];

        assertTrue("getRotationMatrix returns false on valid inputs",
            SensorManager.getRotationMatrix(Rr2, Ir2, gI, mI));

        assertVectorRoughlyEqual(
            "getRotationMatrix acts inconsistent with 9- and 16- elements matrix buffer",
            mat16to9(Rr2), Rr, 1e-4f);

        assertVectorRoughlyEqual(
            "getRotationMatrix acts inconsistent with 9- and 16- elements matrix buffer",
            mat16to9(Ir2), Ir, 1e-4f);

        // test null inputs
        assertTrue("getRotationMatrix does not handle null inputs",
            SensorManager.getRotationMatrix(Rr, null, gI, mI));

        assertTrue("getRotationMatrix does not handle null inputs",
            SensorManager.getRotationMatrix(null, Ir, gI, mI));

        assertTrue("getRotationMatrix does not handle null inputs",
            SensorManager.getRotationMatrix(null, null, gI, mI));

        // test fail cases
        // free fall, if the acc reading is less than 10% of gravity
        gI[0] = gI[1] = gI[2] = data.nextFloat() * gravity * 0.05f; // sqrt(3) * 0.05 < 0.1
         assertFalse("getRotationMatrix does not fail when it supposed to fail (gravity too small)",
            SensorManager.getRotationMatrix(Rr, Ir, gI, mI));

        // wrong input
        assertFalse("getRotationMatrix does not fail when it supposed to fail (singular axis)",
            SensorManager.getRotationMatrix(Rr, Ir, gI, gI));
    }

    public void testGetRotationMatrixFromVector() throws Exception {
        TestDataGenerator data = new TestDataGenerator();

        int i;
        float [] v;
        float [] q = new float[4];

        float [] v3 = new float[3];
        float [] v4 = new float[4];
        float [] v5 = new float[5];
        float [][] vs = new float[][]{v3, v4, v5};

        float [] m9 = new float[9];
        float [] m16 = new float[16];

        // format: x y z theta/2
        float [] xyzth = new float[4];
        // test the orthogonal property of returned matrix
        for (i=0; i<20; ++i) {
            float c, s;
            data.nextRotationAxisAngle(xyzth);

            c = (float) Math.cos(xyzth[3]);
            s = (float) Math.sin(xyzth[3]);
            if (c < 0.f) {
                c = -c;
                s = -s;
            }

            v = vs[i%3];
            switch(i%3) {
                case 2:
                    v[4] = data.nextBoolean() ? data.nextFloat() : -1.f;
                case 1:
                    v[3] = c;
                case 0:
                    v[0] = s * xyzth[0];
                    v[1] = s * xyzth[1];
                    v[2] = s * xyzth[2];
            }

            if ((i % 1) != 0) {
                SensorManager.getRotationMatrixFromVector(m16, v);
                m9 = mat16to9(m16);
            }else {
                SensorManager.getRotationMatrixFromVector(m9, v);
            }

            float [] n = mat9Mul(m9, mat9T(m9));
            assertRoughlyEqual("getRotationMatrixFromVector do not return proper matrix",
                    n[0]+ n[4] + n[8], 3.f, 1e-4f);
        }

        // test if multiple rotation (total 2pi) about an axis result in identity
        v = v3;
        float [] Rr = new float[9];

        for (i=0; i<20; ++i) {
            float j, halfTheta, residualHalfTheta = FLOAT_PI;
            float [] R = mat9I();
            float c, s;

            data.nextRotationAxisAngle(xyzth);  // half theta is ignored

            j = data.nextInt(5) + 2;  // 2 ~ 6 rotations

            while(j-- > 0) {
                if (j == 0) {
                    halfTheta = residualHalfTheta;
                } else {
                    halfTheta = data.nextFloat() * FLOAT_PI;
                }

                c = (float) Math.cos(halfTheta);
                s = (float) Math.sin(halfTheta);
                if (c < 0.f) {
                    c = -c;
                    s = -s;
                }

                v[0] = s * xyzth[0];
                v[1] = s * xyzth[1];
                v[2] = s * xyzth[2];

                SensorManager.getRotationMatrixFromVector(Rr, v);
                R = mat9Mul(Rr, R);

                residualHalfTheta -= halfTheta;
            }

            assertRoughlyEqual("getRotationMatrixFromVector returns incorrect matrix",
                    R[0] + R[4] + R[8], 3.f, 1e-4f);
        }

        // test if rotation about trival axis works
        v = v3;
        for (i=0; i<20; ++i) {
            int axis = (i % 3) + 1;
            float theta = data.nextFloat() * 2.f * FLOAT_PI;
            float [] R;

            v[0] = v[1] = v[2] = 0.f;
            v[axis - 1] = (float) Math.sin(theta / 2.f);
            if ( (float) Math.cos(theta / 2.f) < 0.f) {
                v[axis-1] = -v[axis-1];
            }

            SensorManager.getRotationMatrixFromVector(m9, v);
            R = mat9Rot(axis, theta);

            assertVectorRoughlyEqual(
                String.format("getRotationMatrixFromVector returns incorrect matrix with "+
                    "simple rotation. Details: case %d, truth R = %s, result R = %s.",
                    i, mat9ToStr(R), mat9ToStr(m9)),
                R, m9, 1e-4f);
        }
    }

    public void testRemapCoordinateSystem() throws Exception {
        TestDataGenerator data = new TestDataGenerator();

        int i, j, k;
        float [] rotv = new float[3];
        float [] Rout = new float[9];
        float [] Rout2 = new float[16];
        int a1, a2; // AXIS_X/Y/Z
        int b1, b2, b3; // AXIS_X/Y/Z w/ or w/o MINUS

        // test a few instances
        for (i=0; i<10; ++i) {
            float [] R;
            // yaw pitch roll
            data.nextRotationAngles(rotv);
            R = mat9VRot(rotv);

            // total of 6*4 = 24 variations
            // 6 = A(3,2)
            for (j=0; j<9; ++j) {
                // axis without minus
                a1 = j/3 + 1;
                a2 = j%3 + 1;

                // skip cases when two axis are the same
                if (a1 == a2) continue;

                for (k=0; k<3; ++k) {
                    // test all minus axis combination: ++, +-, -+, --
                    b1 = a1 | (((k & 2) != 0) ? 0x80 : 0);
                    b2 = a2 | (((k & 1) != 0) ? 0x80 : 0);
                    // the third axis
                    b3 = (6 - a1 -a2) |
                         ( (((a2 + 3 - a1) % 3 == 2) ? 0x80 : 0) ^ (b1 & 0x80) ^ (b2 & 0x80));

                    // test both input formats
                    if ( (i & 1) != 0 ) {
                      assertTrue(SensorManager.remapCoordinateSystem(R, b1, b2, Rout));
                    } else {
                      assertTrue(SensorManager.remapCoordinateSystem(mat9to16(R), b1, b2, Rout2));
                      Rout = mat16to9(Rout2);
                    }

                    float [] v1, v2;

                    String detail = String.format(
                            "Details: case %d (%x %x %x), original R = %s, result R = %s.",
                            i, b1, b2, b3, mat9ToStr(R), mat9ToStr(Rout));

                    v1 = mat9Axis(R, SensorManager.AXIS_X);
                    v2 = mat9Axis(Rout, b1);
                    assertVectorRoughlyEqual(
                        "remapCoordinateSystem gives incorrect result (x)." + detail,
                        v1, v2, 1e-4f);

                    v1 = mat9Axis(R, SensorManager.AXIS_Y);
                    v2 = mat9Axis(Rout, b2);
                    assertVectorRoughlyEqual(
                        "remapCoordinateSystem gives incorrect result (y)." + detail,
                        v1, v2, 1e-4f);

                    v1 = mat9Axis(R, SensorManager.AXIS_Z);
                    v2 = mat9Axis(Rout, b3);
                    assertVectorRoughlyEqual(
                        "remapCoordinateSystem gives incorrect result (z)." + detail,
                        v1, v2, 1e-4f);
                }
            }

        }

        // test cases when false should be returned
        assertTrue("remapCoordinateSystem should return false with mismatch size input and output",
                   !SensorManager.remapCoordinateSystem(Rout,
                     SensorManager.AXIS_Y, SensorManager.AXIS_Z, Rout2));
        assertTrue("remapCoordinateSystem should return false with invalid axis setting",
                   !SensorManager.remapCoordinateSystem(Rout,
                     SensorManager.AXIS_X, SensorManager.AXIS_X, Rout));
        assertTrue("remapCoordinateSystem should return false with invalid axis setting",
                   !SensorManager.remapCoordinateSystem(Rout,
                     SensorManager.AXIS_X, SensorManager.AXIS_MINUS_X, Rout));

    }

    // Utilities class & functions

    private class TestDataGenerator {
        // carry out test deterministically without manually picking numbers
        private final long DEFAULT_SEED = 0xFEDCBA9876543210l;

        private Random mRandom;

        TestDataGenerator(long seed) {
            mRandom = new Random(seed);
        }

        TestDataGenerator() {
            mRandom = new Random(DEFAULT_SEED);
        }

        void nextRotationAngles(float [] rotv) {
            assertTrue(rotv.length == 3);

            rotv[0] = (mRandom.nextFloat()-0.5f) * 2.0f * FLOAT_PI; // azimuth(yaw) -pi ~ pi
            rotv[1] = (mRandom.nextFloat()-0.5f) * FLOAT_PI; // pitch -pi/2 ~ +pi/2
            rotv[2] = (mRandom.nextFloat()-0.5f) * 2.f * FLOAT_PI; // roll -pi ~ +pi
        }

        void nextRotationAxisAngle(float [] aa) {
            assertTrue(aa.length == 4);

            aa[0] = (mRandom.nextFloat() - 0.5f) * 2.f;
            aa[1] = (mRandom.nextFloat() - 0.5f ) * 2.f * (float) Math.sqrt(1.f - aa[0] * aa[0]);
            aa[2] = (mRandom.nextBoolean() ? 1.f : -1.f) *
                        (float) Math.sqrt(1.f - aa[0] * aa[0] - aa[1] * aa[1]);
            aa[3] = mRandom.nextFloat() * FLOAT_PI;
        }

        int nextInt(int i) {
            return mRandom.nextInt(i);
        }

        float nextFloat() {
            return mRandom.nextFloat();
        }

        boolean nextBoolean() {
            return mRandom.nextBoolean();
        }
    }

    private static void assertRotationAnglesValid(String message, float[] ra) {

        assertTrue(message, ra.length == 3 &&
            ra[0] >= -FLOAT_PI && ra[0] <= FLOAT_PI &&         // azimuth
            ra[1] >= -FLOAT_PI / 2.f && ra[1] <= FLOAT_PI / 2.f && // pitch
            ra[2] >= -FLOAT_PI && ra[2] <= FLOAT_PI);          // roll
    }

    private static void assertRoughlyEqual(String message, float a, float b, float bound) {
        assertTrue(message, Math.abs(a-b) < bound);
    }

    private static void assertVectorRoughlyEqual(String message, float [] v1, float [] v2,
                                                 float bound) {
        assertTrue(message, v1.length == v2.length);
        int i;
        float sum = 0.f;
        for (i=0; i<v1.length; ++i) {
            sum += (v1[i] - v2[i]) * (v1[i] - v2[i]);
        }
        assertRoughlyEqual(message, (float)Math.sqrt(sum), 0.f, bound);
    }

    private static float [] mat9to16(float [] m) {
        assertTrue(m.length == 9);

        float [] n  = new float[16];
        int i;
        for (i=0; i<9; ++i) {
            n[i+i/3] = m[i];
        }
        n[15] = 1.f;
        return n;
    }

    private static float [] mat16to9(float [] m) {
        assertTrue(m.length == 16);

        float [] n = new float[9];
        int i;
        for (i=0; i<9; ++i) {
            n[i] = m[i + i/3];
        }
        return n;
    }

    private static float [] mat9Mul(float [] m, float [] n) {
        assertTrue(m.length == 9 && n.length == 9);

        float [] r = new float[9];
        int i, j, k;

        for (i = 0; i < 3; ++i)
            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    r[i * 3 + j] += m[i * 3 + k] * n[k * 3 + j];

        return r;
    }

    private static float [] mat9T(float [] m) {
        assertTrue(m.length == 9);

        int i, j;
        float [] n = new float[9];

        for (i = 0; i < 3; ++i)
            for (j = 0; j < 3; ++j)
                n[i * 3 + j] = m[j * 3 + i];

        return n;
    }

    private static float [] mat9I() {
        float [] m = new float[9];
        m[0] = m[4] = m[8] = 1.f;
        return m;
    }

    private static float [] mat9Rot(int axis, float angle) {
        float [] m = new float[9];
        switch (axis) {
            case SensorManager.AXIS_X:
                m[0] = 1.f;
                m[4] = m[8] = (float) Math.cos(angle);
                m[5] = - (m[7] = (float) Math.sin(angle));
                break;
            case SensorManager.AXIS_Y:
                m[4] = 1.f;
                m[0] = m[8] = (float) Math.cos(angle);
                m[6] = - (m[2] = (float) Math.sin(angle));
                break;
            case SensorManager.AXIS_Z:
                m[8] = 1.f;
                m[0] = m[4] = (float) Math.cos(angle);
                m[1] = - (m[3] = (float) Math.sin(angle));
                break;
            default:
                // should never be here
                assertTrue(false);
        }
        return m;
    }

    private static float [] mat9VRot(float [] angles) {
        assertTrue(angles.length == 3);
        // yaw, android yaw rotate to -z
        float [] R = mat9Rot(SensorManager.AXIS_Z, -angles[0]);
        // pitch, android pitch rotate to -x
        R = mat9Mul(R, mat9Rot(SensorManager.AXIS_X, -angles[1]));
        // roll
        R = mat9Mul(R, mat9Rot(SensorManager.AXIS_Y, angles[2]));

        return R;
    }

    private static float [] mat9Axis(float m[], int axis) {
        assertTrue(m.length == 9);

        boolean negative = (axis & 0x80) != 0;
        float [] v = new float[3];
        int offset;

        offset = (axis & ~0x80) - 1;
        v[0] = negative ? -m[offset]   : m[offset];
        v[1] = negative ? -m[offset+3] : m[offset+3];
        v[2] = negative ? -m[offset+6] : m[offset+6];
        return v;
    }

    private static float vecInner(float u[], float v[]) {
        assertTrue(u.length == v.length);

        int i;
        float sum = 0.f;

        for (i=0; i < v.length; ++i) {
            sum += u[i]*v[i];
        }
        return (float)Math.sqrt(sum);
    }

    private static String vecToStr(float u[]) {
        int i;
        String s;
        switch (u.length) {
            case 3:
                return String.format("[%f, %f, %f]", u[0], u[1], u[2]);
            case 4:
                return String.format("(%f, %f, %f, %f)", u[0], u[1], u[2], u[3]);
            default:
                s = "[";
                for (i = 0; i < u.length-1; ++i) {
                    s += String.format("%f, ", u[i]);
                }
                s += String.format("%f]", u[i]);
                return s;
        }
    }

    private static String mat9ToStr(float m[]) {
        assertTrue(m.length == 9);
        return String.format("[%f, %f, %f; %f, %f, %f; %f, %f, %f]",
            m[0], m[1], m[2],
            m[3], m[4], m[5],
            m[6], m[7], m[8]);
    }

    private static String mat16ToStr(float m[]) {
        assertTrue(m.length == 16);
        return String.format("[%f, %f, %f, %f; %f, %f, %f, %f; %f, %f, %f, %f; %f, %f, %f, %f]",
            m[0], m[1], m[2], m[3],
            m[4], m[5], m[6], m[7],
            m[8], m[9], m[10], m[11],
            m[12], m[13], m[14], m[15]);
    }

}