//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

#ifdef NDEBUG
#    undef NDEBUG
#endif

#include <ImathMatrixAlgo.h>
#include <ImathRandom.h>
#include <assert.h>
#include <exception>
#include <iostream>
#include <stdio.h>
#include "testMiscMatrixAlgo.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 * float(M_PI / 180);
}

void
testComputeLocalFrame()
{
    float  eps = 0.00005f;
    Rand48 random (0);
    for (int i = 0; i < 100000; ++i)
    {
        debug (("iteration: %d\n", i));

        // Random pos
        V3f p (
            float(random.nextf (-10, 10)),
            float(random.nextf (-10, 10)),
            float(random.nextf (-10, 10)));

        // Random xDir
        V3f xDir (
            float(random.nextf (-10, 10)),
            float(random.nextf (-10, 10)),
            float(random.nextf (-10, 10)));

        // Random normalDir
        V3f normalDir (
            float(random.nextf (-10, 10)),
            float(random.nextf (-10, 10)),
            float(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) < eps));

        // 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) < eps);
        /*std::cout<<y<<"\n";
        std::cout<<normalDir<<"\n";
        std::cout<<(y^normalDir)<<"\n";*/
        assert (abs (y ^ normalDir) < eps);
        assert ((abs (y.length() - 1.f) < eps));

        // 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) < eps));
        assert (abs (x ^ z) < eps);
        assert (abs (y ^ z) < eps);
        assert (((x % y) ^ z) > 0);
    }
}

void
getRandTRS (Rand48& random, V3f& trans, V3f& rot, V3f& scale)
{
    // Translate
    trans = V3f (
            float(random.nextf (-10, 10)),
            float(random.nextf (-10, 10)),
            float(random.nextf (-10, 10)));
    // Rotate
    rot = V3f (
        rad (float(random.nextf (-180, 180))),
        rad (float(random.nextf (-180, 180))),
        rad (float(random.nextf (-180, 180))));

    // Scale
    V3f s (
        float(random.nextf (0.000001, 2.0)),
        float(random.nextf (0.000001, 2.0)),
        float(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 (
        float(random.nextf (0.000001, 2.0)),
        float(random.nextf (0.000001, 2.0)),
        float(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] += float(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.0001f;

    /// 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="<<D[j][k] << endl;
            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 " << endl;
        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;
    }
}
