/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  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
 *
 *         https://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  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.
 *
 *=========================================================================*/

#include <iostream>

#include "itkMatrix.h"
#include "itkMath.h"
#include "itkTestingMacros.h"

int
itkMatrixTest(int, char *[])
{
  using NumericType = float;
  using MatrixType = itk::Matrix<NumericType, 3, 3>;
  using VectorType = itk::Vector<NumericType, 3>;
  using PointType = itk::Point<NumericType, 3>;
  using CovariantVectorType = itk::CovariantVector<NumericType, 3>;

  using vnlVectorType = vnl_vector_fixed<NumericType, 3>;

  MatrixType matrix;
  matrix.Fill(0.0);
  matrix.SetIdentity();

  VectorType v1;
  v1[0] = 3;
  v1[1] = 4;
  v1[2] = 5;

  VectorType resultVector = matrix * v1;
  std::cout << resultVector[0] << ", ";
  std::cout << resultVector[1] << ", ";
  std::cout << resultVector[2] << std::endl;

  PointType::ValueType p1Init[3] = { 3, 4, 5 };
  PointType            p1 = p1Init;

  PointType resultPoint = matrix * p1;
  std::cout << resultPoint[0] << ", ";
  std::cout << resultPoint[1] << ", ";
  std::cout << resultPoint[2] << std::endl;

  CovariantVectorType::ValueType cv1Init[3] = { 3, 4, 5 };
  CovariantVectorType            cv1 = cv1Init;

  CovariantVectorType resultCovariantVector = matrix * cv1;
  std::cout << resultCovariantVector[0] << ", ";
  std::cout << resultCovariantVector[1] << ", ";
  std::cout << resultCovariantVector[2] << std::endl;

  vnlVectorType v2;
  v2[0] = 3;
  v2[1] = 4;
  v2[2] = 5;

  vnlVectorType resultVnlVector = matrix * v2;
  std::cout << resultVnlVector[0] << ", ";
  std::cout << resultVnlVector[1] << ", ";
  std::cout << resultVnlVector[2] << std::endl;

  MatrixType matrix2;
  matrix2.SetIdentity();
  matrix2.GetVnlMatrix()(0, 0) = 10;

  MatrixType matrixProduct;
  matrixProduct = matrix * matrix2;

  MatrixType matrix3;
  matrix3 = matrix.GetInverse();

  MatrixType matrix4;
  matrix4 = matrix.GetTranspose();

  MatrixType matrix5;
  matrix5.Fill(1.7);

  constexpr NumericType value = 2;
  matrix5[1][1] = value;
  if (itk::Math::NotExactlyEquals(matrix5[1][1], value))
  {
    std::cerr << "Problem accessing matrix element " << std::endl;
    return EXIT_FAILURE;
  }

  // Test access with the operator()(row,col)
  constexpr NumericType value2 = 19;
  matrix5(1, 1) = value2;
  if (itk::Math::NotExactlyEquals(matrix5[1][1], value2))
  {
    std::cerr << "Problem accessing matrix element " << std::endl;
    return EXIT_FAILURE;
  }
  if (itk::Math::NotExactlyEquals(matrix5(1, 1), value2))
  {
    std::cerr << "Problem accessing matrix element " << std::endl;
    return EXIT_FAILURE;
  }


  MatrixType matrix6 = matrix5 * 2.5;

  matrix6 *= 1.3;


  // This was added after a bug in operator*() was reported on the users list.
  std::cout << "Testing products in non-square matrices" << std::endl;

  try
  {
    itk::Matrix<double, 2, 2> m1;
    itk::Matrix<double, 2, 2> m2;

    for (unsigned int i = 0; i < 2; ++i)
    {
      for (unsigned int j = 0; j < 2; ++j)
      {
        m1[i][j] = i + j;
      }
    }

    std::cout << "m1=" << std::endl;
    std::cout << m1 << std::endl;


    for (unsigned int i = 0; i < 2; ++i)
    {
      for (unsigned int j = 0; j < 2; ++j)
      {
        m2[i][j] = i + j;
      }
    }

    std::cout << "m2=" << std::endl;
    std::cout << m2 << std::endl;


    std::cout << "VNL * VNL Multiplication result: " << std::endl;
    std::cout << m1.GetVnlMatrix() * m2.GetVnlMatrix() << std::endl;

    std::cout << "ITK * VNL Multiplication result: " << std::endl;
    std::cout << m1 * m2.GetVnlMatrix() << std::endl;

    itk::Matrix<double, 2, 2> m3;
    itk::Matrix<double, 2, 3> m4;

    for (unsigned int i = 0; i < 2; ++i)
    {
      for (unsigned int j = 0; j < 2; ++j)
      {
        m3[i][j] = i + j;
      }
    }

    std::cout << "m3=" << std::endl;
    std::cout << m3 << std::endl;


    for (unsigned int i = 0; i < 2; ++i)
    {
      for (unsigned int j = 0; j < 3; ++j)
      {
        m4[i][j] = i + j;
      }
    }

    std::cout << "m4=" << std::endl;
    std::cout << m4 << std::endl;


    std::cout << "VNL * VNL Multiplication result: " << std::endl;
    std::cout << m3.GetVnlMatrix() * m4.GetVnlMatrix() << std::endl;

    std::cout << "ITK * VNL Multiplication result: " << std::endl;
    std::cout << m3 * m4.GetVnlMatrix() << std::endl;
  }
  catch (const itk::ExceptionObject & e)
  {
    std::cerr << "Exception caught in test..." << std::endl;
    std::cerr << e << std::endl;
    return EXIT_FAILURE;
  }


  { // Test for Matrix addition and subtraction
    constexpr unsigned int nc = 4;
    constexpr unsigned int nr = 3;

    using AddSubtractMatrixType = itk::Matrix<double, nr, nc>;

    AddSubtractMatrixType m1;
    AddSubtractMatrixType m2;

    // fill the matrices with something
    {
      for (unsigned int r = 0; r < nr; ++r)
      {
        for (unsigned int c = 0; c < nc; ++c)
        {
          const auto fr = static_cast<double>(r);
          const auto fc = static_cast<double>(c);
          m1[r][c] = fr + fc;
          m2[r][c] = fr - fc;
        }
      }
    }

    AddSubtractMatrixType m3;
    m3 = m1 + m2;

    AddSubtractMatrixType m4;
    m4 = m1 - m2;

    std::cout << "Results of ITK matrix addition" << std::endl;
    std::cout << "M1 = " << std::endl << m1 << std::endl;
    std::cout << "M2 = " << std::endl << m2 << std::endl;
    std::cout << "M1+M2 = " << std::endl << m3 << std::endl;
    std::cout << "M1-M2 = " << std::endl << m4 << std::endl;

    // Check the addition and subtraction values
    {
      const double tolerance = 1e-7;
      for (unsigned int r = 0; r < nr; ++r)
      {
        for (unsigned int c = 0; c < nc; ++c)
        {
          if (itk::Math::abs(m3[r][c] - 2 * r) > tolerance)
          {
            std::cerr << "Addition failed !" << std::endl;
            std::cerr << "M[" << r << "][" << c << "] = ";
            std::cerr << m3[r][c] << std::endl;
            return EXIT_FAILURE;
          }
          if (itk::Math::abs(m4[r][c] - 2 * c) > tolerance)
          {
            std::cerr << "Subtraction failed !" << std::endl;
            return EXIT_FAILURE;
          }
        }
      }
    }

    m3 -= m2;
    m4 += m2;

    // Check the in-place addition and subtraction values
    {
      const double tolerance = 1e-7;
      for (unsigned int r = 0; r < nr; ++r)
      {
        for (unsigned int c = 0; c < nc; ++c)
        {
          if (itk::Math::abs(m3[r][c] - m1[r][c]) > tolerance)
          {
            std::cerr << "In-place addition failed !" << std::endl;
            std::cerr << m3 << std::endl;
            return EXIT_FAILURE;
          }
          if (itk::Math::abs(m4[r][c] - m1[r][c]) > tolerance)
          {
            std::cerr << "In-place subtraction failed !" << std::endl;
            std::cerr << m4 << std::endl;
            return EXIT_FAILURE;
          }
        }
      }
    }
  }

  {
    // Test for vnl_matrix_fixed assignment and construction
    MatrixType matrixA;

    int counter = 0;
    for (unsigned int row = 0; row < 3; ++row)
    {
      for (unsigned int col = 0; col < 3; ++col)
      {
        matrixA[row][col] = counter++;
      }
    }

    MatrixType::InternalMatrixType vnlMatrixA = matrixA.GetVnlMatrix();

    MatrixType matrixB(vnlMatrixA); // Test constructor

    { // verify values
      const double tolerance = 1e-7;
      for (unsigned int row = 0; row < 3; ++row)
      {
        for (unsigned int col = 0; col < 3; ++col)
        {
          if (itk::Math::abs(matrixB[row][col] - matrixA[row][col]) > tolerance)
          {
            std::cerr << "constructor from vnl_matrix failed ! " << std::endl;
            return EXIT_FAILURE;
          }
        }
      }
    }

    MatrixType matrixC;
    matrixC = vnlMatrixA; // Test assignment

    { // verify values
      const double tolerance = 1e-7;
      for (unsigned int row = 0; row < 3; ++row)
      {
        for (unsigned int col = 0; col < 3; ++col)
        {
          if (itk::Math::abs(matrixC[row][col] - matrixA[row][col]) > tolerance)
          {
            std::cerr << "assignment from vnl_matrix failed ! " << std::endl;
            return EXIT_FAILURE;
          }
        }
      }
    }
  }

  using LargeMatrixType = itk::Matrix<NumericType, 7, 7>;
  LargeMatrixType matrixBad;
  matrixBad.Fill(2.0);
  ITK_TRY_EXPECT_EXCEPTION(matrixBad.GetInverse());

  matrixBad.SetIdentity();
  ITK_TRY_EXPECT_NO_EXCEPTION(matrixBad.GetInverse());

  {
    /*
     *  This tries to invert a matrix close to
     *     1.0156e+00   3.7242e-33   1.8034e-17
     *    -1.5263e-17   2.4781e-16   1.2000e+00
     *    -1.4363e-40  -1.0156e+00   2.9279e-16
     *
     *  Matlab suggest that the inverse of this matrix is close to
     *
     *     9.8462e-01  -1.4797e-17  -5.6092e-40
     *     3.6105e-33   2.4024e-16  -9.8462e-01
     *     1.2524e-17   8.3334e-01   2.0333e-16
     *
     *  However, it has been reported that vnl_matrix_inverse may produce a warning
     *
     *    vnl/algo/vnl_svd.txx: suspicious return value (3) from SVDC
     *    vnl/algo/vnl_svd.txx: M is 3x3
     *
     *  and will then only return NaN values
     *
     */

    itk::Matrix<double, 3, 3> invertibleMatrix;
    invertibleMatrix[0][0] = 1.015625;
    invertibleMatrix[0][1] = 3.7241876106217399000472402087113071242494464386448e-33;
    invertibleMatrix[0][2] = 1.8034176825630083413048304619152872874110471457243e-17;
    invertibleMatrix[1][0] = -1.5263338494897929595479901809795819644932635128498e-17;
    invertibleMatrix[1][1] = 2.4780806492783651398537081433914863737300038337708e-16;
    invertibleMatrix[1][2] = 1.1999969482421875;
    invertibleMatrix[2][0] = -1.4362888869790077531846951109846653370783300825652e-40;
    invertibleMatrix[2][1] = -1.015625;
    invertibleMatrix[2][2] = 2.9279401122629200669017501823532256821636110544205e-16;

    itk::Matrix<double, 3, 3> inverseMatrixMatlab;
    inverseMatrixMatlab[0][0] = 9.8462e-01;
    inverseMatrixMatlab[0][1] = -1.4797e-17;
    inverseMatrixMatlab[0][2] = -5.6092e-40;
    inverseMatrixMatlab[1][0] = 3.6105e-33;
    inverseMatrixMatlab[1][1] = 2.4024e-16;
    inverseMatrixMatlab[1][2] = -9.8462e-01;
    inverseMatrixMatlab[2][0] = 1.2524e-17;
    inverseMatrixMatlab[2][1] = 8.3334e-01;
    inverseMatrixMatlab[2][2] = 2.0333e-16;

    std::cout << "Testing matrix inversion for " << std::endl << invertibleMatrix << std::endl;

    const vnl_matrix_fixed<double, 3, 3> invertedMatrix = invertibleMatrix.GetInverse();

    std::cout << "Inverted to " << std::endl << invertedMatrix << std::endl;

    std::cout << "Matlab inverse " << std::endl << inverseMatrixMatlab << std::endl;

    unsigned int num_nans(0);
    for (unsigned int i = 0; i < 3; ++i)
    {
      for (unsigned int j = 0; j < 3; ++j)
      {
        if (itk::Math::isnan(invertedMatrix[i][j]))
        {
          ++num_nans;
        }
      }
    }


    const double relative_error = (invertedMatrix - inverseMatrixMatlab.GetVnlMatrix()).frobenius_norm() /
                                  inverseMatrixMatlab.GetVnlMatrix().frobenius_norm();

    std::cout << "Relative error compared to Matlab " << std::endl << relative_error << std::endl;

    if (num_nans > 0 || relative_error > 1e-4)
    {
      std::cout << "Matrix could not be inverted" << std::endl;
      return EXIT_FAILURE;
    }
  }


  std::cout << "Test Passed !" << std::endl;
  return EXIT_SUCCESS;
}
