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

#ifdef NDEBUG
#    undef NDEBUG
#endif

#include <ImathMatrix.h>
#include <ImathMatrixAlgo.h>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <ctime>
#include <iostream>
#include <limits>
#include <math.h>

using namespace std;
using namespace IMATH_INTERNAL_NAMESPACE;

// clang-format off

const Matrix33<double> A33_1 ( 1, 0, 0, 0, 1, 0, 0, 0, 1 );
const Matrix33<double> A33_2 ( 1, 0, 0, 0,-1, 0, 0, 0, 1 );
const Matrix33<double> A33_3 ( 1, 0, 0, 0, 1, 0, 0, 0, 0 );
const Matrix33<double> A33_4 ( 1, 0, 0, 0, 0, 0, 0, 0, 0 );
const Matrix33<double> A33_5 ( 0, 0, 0, 0, 0, 0, 0, 0, 0 );
const Matrix33<double> A33_6 ( 1, 0, 0, 0, 1e-10, 0, 0, 0, 0 );
const Matrix33<double> A33_7 ( 1, 0, 0, 0, 1e-10, 0, 0, 0, 1e+10 );
const Matrix33<double> A33_8 (
     0.25058694044821,  0.49427229444416,  0.81415724537748,
     0.49427229444416,  0.80192384710853, -0.61674948224910,
     0.81415724537748, -0.61674948224910, -1.28486154645285);

const Matrix44<double> A44_1 ( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 );
const Matrix44<double> A44_2 ( 1, 0, 0, 0, 0,-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 );
const Matrix44<double> A44_3 ( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 );
const Matrix44<double> A44_4 ( 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 );
const Matrix44<double> A44_5 ( 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 );
const Matrix44<double> A44_6 ( 1, 0, 0, 0, 0, 1e-20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
const Matrix44<double> A44_7 ( 1, 0, 0, 0, 0, 1e-20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e+20 );
const Matrix44<double> A44_8 (
     4.05747631538951,  0.16358123075600,  0.11541756047409, -1.65369223465270,
     0.16358123075600,  0.57629829390780,  3.88542912704029,  0.92016316185369,
     0.11541756047409,  3.88542912704029,  0.65367032943707, -0.21971103270410,
    -1.65369223465270,  0.92016316185369, -0.21971103270410, -0.28108876552761);

// clang-format on

template <typename TM>
void
verifyOrthonormal (const TM& A, const typename TM::BaseType threshold)
{
    const TM prod = A * A.transposed ();
    for (unsigned int i = 0; i < TM::dimensions (); ++i)
        for (unsigned int j = 0; j < TM::dimensions (); ++j)
            if (i == j)
                assert (std::abs (prod[i][j] - 1) < threshold);
            else
                assert (std::abs (prod[i][j]) < threshold);
}

template <typename TM>
typename TM::BaseType
computeThreshold (const TM& A)
{
    typedef typename TM::BaseType T;
    T                             maxAbsEntry (0);

    for (unsigned int i = 0; i < TM::dimensions (); ++i)
        for (unsigned int j = 0; j < TM::dimensions (); ++j)
            maxAbsEntry = std::max (maxAbsEntry, std::abs (A[i][j]));

    const T eps = std::numeric_limits<T>::epsilon ();
    maxAbsEntry = std::max (maxAbsEntry, eps);

    return maxAbsEntry * T (100) * eps;
}

template <class TM>
void
testJacobiEigenSolver (const TM& A)
{
    using std::abs;

    typedef typename TM::BaseType    T;
    typedef typename TM::BaseVecType TV;

    const T threshold = computeThreshold (A);

    TM AA (A);
    TV S;
    TM V;

    jacobiEigenSolver (AA, S, V);

    // Orthogonality of V
    verifyOrthonormal (V, threshold);

    // Determinant of V
    assert (abs (V.determinant ()) - 1 < threshold);

    // Determinant of A and S
    TM MS;
    for (unsigned int i = 0; i < TM::dimensions (); ++i)
        for (unsigned int j = 0; j < TM::dimensions (); ++j)
            if (i == j)
                MS[i][j] = S[i];
            else
                MS[i][j] = 0;

    assert (abs (A.determinant ()) - abs (MS.determinant ()) < threshold);

    // A = V * S * V^T
    TM MA = V * MS * V.transposed ();

    for (unsigned int i = 0; i < TM::dimensions (); ++i)
        for (unsigned int j = 0; j < TM::dimensions (); ++j)
            assert (abs (A[i][j] - MA[i][j]) < threshold);
}

template <class TM>
void
testMinMaxEigenValue (const TM& A)
{
    typedef typename TM::BaseVecType TV;
    typedef typename TM::BaseType    T;

    TV minV, maxV, S;
    TM U, V;

    const T threshold = computeThreshold (A);

    {
        TM A1 (A);
        minEigenVector (A1, minV);
        TM A2 (A);
        maxEigenVector (A2, maxV);
    }
    {
        TM A3 (A);
        jacobiSVD (A3, U, S, V);
    }

    const int dim = TM::dimensions ();

    for (int i = 0; i < dim; ++i)
    {
        assert (abs (minV[i] - V[i][dim - 1]) < threshold);
        assert (abs (maxV[i] - V[i][0]) < threshold);
    }
}

template <class T>
void
testJacobiTiming ()
{

    int     rounds (100000);
    clock_t tJacobi, tSVD, t;

    {
        Matrix33<T> A, V, U;
        Vec3<T>     S;

        t = clock ();
        for (int i = 0; i < rounds; ++i)
        {
            A = Matrix33<T> (A33_7);
            jacobiEigenSolver (A, S, V);
            A = Matrix33<T> (A33_8);
            jacobiEigenSolver (A, S, V);
        }
        tJacobi = clock () - t;
        cout << "Jacobi EigenSolver of 3x3 matrices took " << tJacobi
             << " clocks." << endl;

        t = clock ();
        for (int i = 0; i < rounds; ++i)
        {
            A = Matrix33<T> (A33_7);
            jacobiSVD (A, U, S, V);
            A = Matrix33<T> (A33_8);
            jacobiSVD (A, U, S, V);
        }
        tSVD = clock () - t;
        cout << "TinySVD            of 3x3 matrices took " << tSVD << " clocks."
             << endl;
        cout << (float) (tSVD - tJacobi) * 100.0f / (float) (tSVD)
             << "% speed up." << endl;
    }

    {
        Matrix44<T> A, V, U;
        Vec4<T>     S;

        t = clock ();
        for (int i = 0; i < rounds; ++i)
        {
            A = Matrix44<T> (A44_7);
            jacobiEigenSolver (A, S, V);
            A = Matrix44<T> (A44_8);
            jacobiEigenSolver (A, S, V);
        }
        tJacobi = clock () - t;
        cout << "Jacobi EigenSolver of 4x4 matrices took " << tJacobi
             << " clocks" << endl;

        t = clock ();
        for (int i = 0; i < rounds; ++i)
        {
            A = Matrix44<T> (A44_7);
            jacobiSVD (A, U, S, V);
            A = Matrix44<T> (A44_8);
            jacobiSVD (A, U, S, V);
        }
        tSVD = clock () - t;
        cout << "TinySVD            of 4x4 matrices took " << tSVD << " clocks"
             << endl;
        cout << (float) (tSVD - tJacobi) * 100.0f / (float) (tSVD)
             << "% speed up." << endl;
    }
}

template <class T>
void
testJacobiEigenSolverImp ()
{
    testJacobiEigenSolver (Matrix33<T> (A33_1));
    testJacobiEigenSolver (Matrix33<T> (A33_2));
    testJacobiEigenSolver (Matrix33<T> (A33_3));
    testJacobiEigenSolver (Matrix33<T> (A33_4));
    testJacobiEigenSolver (Matrix33<T> (A33_5));
    testJacobiEigenSolver (Matrix33<T> (A33_6));
    testJacobiEigenSolver (Matrix33<T> (A33_7));
    testJacobiEigenSolver (Matrix33<T> (A33_8));

    testJacobiEigenSolver (Matrix44<T> (A44_1));
    testJacobiEigenSolver (Matrix44<T> (A44_2));
    testJacobiEigenSolver (Matrix44<T> (A44_3));
    testJacobiEigenSolver (Matrix44<T> (A44_4));
    testJacobiEigenSolver (Matrix44<T> (A44_5));
    testJacobiEigenSolver (Matrix44<T> (A44_6));
    testJacobiEigenSolver (Matrix44<T> (A44_7));
    testJacobiEigenSolver (Matrix44<T> (A44_8));
}

template <class T>
void
testMinMaxEigenValueImp ()
{
    testMinMaxEigenValue (Matrix33<T> (A33_7));
    testMinMaxEigenValue (Matrix33<T> (A33_8));

    testMinMaxEigenValue (Matrix44<T> (A44_7));
    testMinMaxEigenValue (Matrix44<T> (A44_8));
}

void
testJacobiEigenSolver ()
{
    cout << endl;
    cout
        << "************ Testing IMATH_INTERNAL_NAMESPACE::ImathJacobiEigenSolver ************"
        << endl;

    cout << "Jacobi EigenSolver in single precision...";
    testJacobiEigenSolverImp<float> ();
    cout << "PASS" << endl;

    cout << "Jacobi EigenSolver in double precision...";
    testJacobiEigenSolverImp<double> ();
    cout << "PASS" << endl;

    cout << "Min/Max EigenValue in single precision...";
    testMinMaxEigenValueImp<float> ();
    cout << "PASS" << endl;

    cout << "Min/Max EigenValue in double precision...";
    testMinMaxEigenValueImp<double> ();
    cout << "PASS" << endl;

    cout << "Timing Jacobi EigenSolver in single precision...\n";
    testJacobiTiming<float> ();

    cout << "Timing Jacobi EigenSolver in double precision...\n";
    testJacobiTiming<double> ();

    cout << "************      ALL PASS          ************" << endl;
}
