///////////////////////////////////////////////////////////////////////////

//

// Copyright (c) 2009, Industrial Light & Magic, a division of Lucas

// Digital Ltd. LLC

// 

// All rights reserved.

// 

// Redistribution and use in source and binary forms, with or without

// modification, are permitted provided that the following conditions are

// met:

// *       Redistributions of source code must retain the above copyright

// notice, this list of conditions and the following disclaimer.

// *       Redistributions in binary form must reproduce the above

// copyright notice, this list of conditions and the following disclaimer

// in the documentation and/or other materials provided with the

// distribution.

// *       Neither the name of Industrial Light & Magic nor the names of

// its contributors may be used to endorse or promote products derived

// from this software without specific prior written permission. 

// 

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT

// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT

// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,

// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY

// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

//

///////////////////////////////////////////////////////////////////////////

#include <testMiscMatrixAlgo.h>

#include "ImathMatrixAlgo.h"

#include "ImathRandom.h"

#include <iostream>

#include <exception>

#include <stdio.h>

#include <assert.h>

#if 0

    #define debug(x) (printf x, fflush (stdout))

#else

    #define debug(x)

#endif

using namespace std;

using namespace IMATH_INTERNAL_NAMESPACE;

namespace {

float rad (float deg) {return deg * (M_PI / 180);}

void

testComputeLocalFrame ()

{

    float eps = 0.00005;

    Rand48 random(0);

    for (int i = 0; i < 100000; ++i)

    {

        debug (("iteration: %d\n", i));

        // Random pos

        V3f p (random.nextf (-10, 10), 

               random.nextf (-10, 10), 

               random.nextf (-10, 10));

        // Random xDir 

        V3f xDir (random.nextf (-10, 10), 

                  random.nextf (-10, 10), 

                  random.nextf (-10, 10));

        // Random normalDir

        V3f normalDir (random.nextf (-10, 10), 

                       random.nextf (-10, 10), 

                       random.nextf (-10, 10));

        // Run computeLocalFrame we want to test

        M44f L = computeLocalFrame(p, xDir, normalDir);

        // test position

        for (int j=0; j<3; j++)

        {

            if ( abs(L[3][j] - p[j])>eps )

                assert(false);

        }

        if (abs (L[3][3] - 1.0)>eps)

            assert(false );

        // check that xAxis has the same dir as xDir and that is is normalized

        V3f x( L[0][0], L[0][1], L[0][2]);

        assert( (x%xDir).length() < eps );

        if (abs (L[0][3])>eps)

            assert(false);

        assert((abs(x.length()-1.f)

        // Check that y is normal to x and to normalDir, and is normalized

        V3f y( L[1][0], L[1][1], L[1][2]);

        if (abs (L[1][3])>eps)

            assert(false );

        assert(abs(x^y)

        /*std::cout<

        std::cout<

        std::cout<<(y^normalDir)<<"\n";*/

        assert(abs(y^normalDir)

        assert((abs(y.length()-1.f)

        // check that z is normalized, normal to x and y, and direct

        V3f z( L[2][0], L[2][1], L[2][2]);

        if (abs (L[2][3])>eps)

            assert(false );

        assert((abs(z.length()-1.f)

        assert(abs(x^z)

        assert(abs(y^z)

        assert(((x%y)^z)>0);

    }

}

void

getRandTRS(Rand48& random, V3f& trans, V3f& rot, V3f& scale)

{

    // Translate 

    trans = V3f (random.nextf (-10, 10), 

                 random.nextf (-10, 10), 

                 random.nextf (-10, 10));

    // Rotate 

    rot = V3f (rad (random.nextf (-180, 180)),

               rad (random.nextf (-180, 180)),

               rad (random.nextf (-180, 180)));

    // Scale 

    V3f s(random.nextf (0.000001, 2.0),

          random.nextf (0.000001, 2.0),

          random.nextf (0.000001, 2.0));

    for (int j=0; j < 3; j++)

        if (random.nextf (0.0, 1.0) >= 0.5)

            s[j] *= -1;

    scale = s;

}

M44f

createRandomMat(Rand48& random, V3f& trans, V3f& rot, V3f& scale)

{

    M44f M;

    V3f t, r, s;

    getRandTRS(random, t, r, s);

    M.translate (t);

    M.rotate (r);

    // Shear M.

    V3f h (random.nextf (0.000001, 2.0), 

           random.nextf (0.000001, 2.0), 

           random.nextf (0.000001, 2.0));

    for (int j=0; j < 3; j++)

        if (random.nextf (0.0, 1.0) >= 0.5)

            h[j] *= -1;

    M.shear (h);

    M.scale (s);

    //

    // Add a small random error to the elements of M

    //

    for (int j = 0; j < 4; ++j)

        for (int k = 0; k < 3; ++k)

            M[j][k] += random.nextf (-1e-7, 1e-7);

    V3f sh;

    extractSHRT (M, scale, sh, rot, trans);

    debug (("Scale   : %f %f %f\n", s[0], s[1], s[2]));

    debug (("Shear   : %f %f %f\n", h[0], h[1], h[2]));

    debug (("Rot     : %f %f %f\n", r[0], r[1], r[2]));

    debug (("Trans   : %f %f %f\n", t[0], t[1], t[2]));

    return M;

}

void

compareMat(M44f& M, M44f& N)

{

    float eps = 0.0001;

    /// Verify that the entries in M and N do not

    // differ too much.

    M44f D (M - N);

    for (int j = 0; j < 4; ++j)

    {

        for (int k = 0; k < 4; ++k)

        {

            //cout << "diff="<

            if (abs (D[j][k]) > eps)

            {

                cout << "unexpectedly diff "<<

                D[j][k] << endl;

                cout << j << " " << k << endl;

                cout << "M\n" << M << endl;

                cout << "N\n" << N << endl;

                cout << "D\n" << D << endl;

                assert (false);

            }

        }

    }

}

void

testAddOffset()

{

    Rand48 random(0);

    for (int i = 0; i < 100000; ++i)

    {

        debug (("iteration: %d\n", i));

        V3f transA, transB, rotA, rotB, scaleA, scaleB;

        V3f tOffset, rOffset, sOffset;

        M44f inMat  = createRandomMat(random, transA, rotA, scaleA);

        M44f refMat = createRandomMat(random, transB, rotB, scaleB);

        getRandTRS(random, tOffset, rOffset, sOffset);

        // addOffset : function to test

        M44f outMat = addOffset( inMat, tOffset, rOffset, sOffset, refMat);

        // add the inverse offset

        M44f invO;

        invO.rotate (V3f(rad(rOffset[0]), rad(rOffset[1]), rad(rOffset[2])));

        invO[3][0] = tOffset[0];

        invO[3][1] = tOffset[1];

        invO[3][2] = tOffset[2];

        invO.invert();

        M44f invS;

        invS.scale (sOffset);

        invS.invert(); // zero scale is avoided in getRandTRS

        // in ref mat from the function result

        M44f outInRefMat = invO*invS*outMat;

        // in ref mat from the inputs

        M44f inRefMat    = inMat*refMat;

        // compare the mat

        compareMat(outInRefMat, inRefMat);

    }

}

void

testRSMatrix(M44f& M, V3f& t, V3f& r, V3f& s)

{

    M44f N;

    N.makeIdentity();

    N.translate (t); // ... matrix compositions

    N.rotate (r);

    N.scale (s);

    compareMat(M, N);

}

void

testComputeRSMatrix ()

{

    Rand48 random(0);

    for (int i = 0; i < 100000; ++i)

    {

        debug (("iteration: %d\n", i));

        V3f transA, transB, rotA, rotB, scaleA, scaleB;

        M44f A = createRandomMat(random, transA, rotA, scaleA);

        M44f B = createRandomMat(random, transB, rotB, scaleB);

        M44f ArAsA = computeRSMatrix( true,  true,  A, B);

        M44f ArBsB = computeRSMatrix( false, false, A, B);

        M44f ArAsB = computeRSMatrix( true,  false, A, B);

        M44f ArBsA = computeRSMatrix( false, true,  A, B);

        testRSMatrix(ArAsA, transA, rotA, scaleA);

        testRSMatrix(ArBsB, transA, rotB, scaleB);

        testRSMatrix(ArAsB, transA, rotA, scaleB);

        testRSMatrix(ArBsA, transA, rotB, scaleA);

        debug (("\n"));

    }

}

} // namespace

void

testMiscMatrixAlgo ()

{

    try

    {

        cout << "Testing misc functions in ImathMatrixAlgo.h" << endl;

        cout << "Testing the building of an orthonormal direct frame from : a position, " 

        << "an x axis direction and a normal to the y axis" << endl;	

        cout << "IMATH_INTERNAL_NAMESPACE::computeLocalFrame()" << endl;

        testComputeLocalFrame ();

        cout << "ok\n" << endl;

        cout << "Add a translate/rotate/scale offset to an input frame "

        << "and put it in another frame of reference" << endl;

        cout << "IMATH_INTERNAL_NAMESPACE::addOffset()" << endl;

        testAddOffset ();

        cout << "ok\n" << endl;

        cout << "Compute Translate/Rotate/Scale matrix from matrix A "<

        cout << "with the Rotate/Scale of Matrix B"<< endl;

        cout << "IMATH_INTERNAL_NAMESPACE::computeRSMatrix()" << endl;

        testComputeRSMatrix ();

        cout << "ok\n" << endl;

    }

    catch (std::exception &e)

    {

        cerr << "  Caught exception: " << e.what () << endl;

    }

}