text: // Ceres Solver - A fast non-linear least squares minimizer
  // Copyright 2015 Google Inc. All rights reserved.
  // http://ceres-solver.org/
  //
  // 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 Google Inc. 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.
  //
  // Author: keir@google.com (Keir Mierle)
  
  #include "ceres/small_blas.h"
  
  #include <limits>
  #include <string>
  
  #include "ceres/internal/eigen.h"
  #include "gtest/gtest.h"
  
  namespace ceres {
  namespace internal {
  
  const double kTolerance = 5.0 * std::numeric_limits<double>::epsilon();
  
  // Static or dynamic problem types.
  enum class DimType { Static, Dynamic };
  
  // Constructs matrix functor type.
  #define MATRIX_FUN_TY(FN)                                                      \
    template <int kRowA, int kColA, int kRowB, int kColB, int kOperation,        \
              DimType kDimType>                                                  \
    struct FN##Ty {                                                              \
      void operator()(const double *A, const int num_row_a, const int num_col_a, \
                      const double *B, const int num_row_b, const int num_col_b, \
                      double *C, const int start_row_c, const int start_col_c,   \
                      const int row_stride_c, const int col_stride_c) {          \
        if (kDimType == DimType::Static) {                                       \
          FN<kRowA, kColA, kRowB, kColB, kOperation>(                            \
              A, num_row_a, num_col_a, B, num_row_b, num_col_b, C, start_row_c,  \
              start_col_c, row_stride_c, col_stride_c);                          \
        } else {                                                                 \
          FN<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic,     \
             kOperation>(A, num_row_a, num_col_a, B, num_row_b, num_col_b, C,    \
                         start_row_c, start_col_c, row_stride_c, col_stride_c);  \
        }                                                                        \
      }                                                                          \
    };
  
  MATRIX_FUN_TY(MatrixMatrixMultiply)
  MATRIX_FUN_TY(MatrixMatrixMultiplyNaive)
  MATRIX_FUN_TY(MatrixTransposeMatrixMultiply)
  MATRIX_FUN_TY(MatrixTransposeMatrixMultiplyNaive)
  
  #undef MATRIX_FUN_TY
  
  // Initializes matrix entires.
  void initMatrix(Matrix &mat) {
    for (int i = 0; i < mat.rows(); ++i) {
      for (int j = 0; j < mat.cols(); ++j) {
        mat(i, j) = i + j + 1;
      }
    }
  }
  
  template <int kRowA, int kColA, int kColB, DimType kDimType,
            template <int, int, int, int, int, DimType> class FunctorTy>
  struct TestMatrixFunctions {
    void operator()() {
      Matrix A(kRowA, kColA);
      initMatrix(A);
      const int kRowB = kColA;
      Matrix B(kRowB, kColB);
      initMatrix(B);
  
      for (int row_stride_c = kRowA; row_stride_c < 3 * kRowA; ++row_stride_c) {
        for (int col_stride_c = kColB; col_stride_c < 3 * kColB; ++col_stride_c) {
          Matrix C(row_stride_c, col_stride_c);
          C.setOnes();
  
          Matrix C_plus = C;
          Matrix C_minus = C;
          Matrix C_assign = C;
  
          Matrix C_plus_ref = C;
          Matrix C_minus_ref = C;
          Matrix C_assign_ref = C;
  
          for (int start_row_c = 0; start_row_c + kRowA < row_stride_c;
               ++start_row_c) {
            for (int start_col_c = 0; start_col_c + kColB < col_stride_c;
                 ++start_col_c) {
              C_plus_ref.block(start_row_c, start_col_c, kRowA, kColB) += A * B;
              FunctorTy<kRowA, kColA, kRowB, kColB, 1, kDimType>()(
                  A.data(), kRowA, kColA, B.data(), kRowB, kColB, C_plus.data(),
                  start_row_c, start_col_c, row_stride_c, col_stride_c);
  
              EXPECT_NEAR((C_plus_ref - C_plus).norm(), 0.0, kTolerance)
                  << "C += A * B \n"
                  << "row_stride_c : " << row_stride_c << "\n"
                  << "col_stride_c : " << col_stride_c << "\n"
                  << "start_row_c  : " << start_row_c << "\n"
                  << "start_col_c  : " << start_col_c << "\n"
                  << "Cref : \n"
                  << C_plus_ref << "\n"
                  << "C: \n"
                  << C_plus;
  
              C_minus_ref.block(start_row_c, start_col_c, kRowA, kColB) -= A * B;
              FunctorTy<kRowA, kColA, kRowB, kColB, -1, kDimType>()(
                  A.data(), kRowA, kColA, B.data(), kRowB, kColB, C_minus.data(),
                  start_row_c, start_col_c, row_stride_c, col_stride_c);
  
              EXPECT_NEAR((C_minus_ref - C_minus).norm(), 0.0, kTolerance)
                  << "C -= A * B \n"
                  << "row_stride_c : " << row_stride_c << "\n"
                  << "col_stride_c : " << col_stride_c << "\n"
                  << "start_row_c  : " << start_row_c << "\n"
                  << "start_col_c  : " << start_col_c << "\n"
                  << "Cref : \n"
                  << C_minus_ref << "\n"
                  << "C: \n"
                  << C_minus;
  
              C_assign_ref.block(start_row_c, start_col_c, kRowA, kColB) = A * B;
  
              FunctorTy<kRowA, kColA, kRowB, kColB, 0, kDimType>()(
                  A.data(), kRowA, kColA, B.data(), kRowB, kColB, C_assign.data(),
                  start_row_c, start_col_c, row_stride_c, col_stride_c);
  
              EXPECT_NEAR((C_assign_ref - C_assign).norm(), 0.0, kTolerance)
                  << "C = A * B \n"
                  << "row_stride_c : " << row_stride_c << "\n"
                  << "col_stride_c : " << col_stride_c << "\n"
                  << "start_row_c  : " << start_row_c << "\n"
                  << "start_col_c  : " << start_col_c << "\n"
                  << "Cref : \n"
                  << C_assign_ref << "\n"
                  << "C: \n"
                  << C_assign;
            }
          }
        }
      }
    }
  };
  
  template <int kRowA, int kColA, int kColB, DimType kDimType,
            template <int, int, int, int, int, DimType> class FunctorTy>
  struct TestMatrixTransposeFunctions {
    void operator()() {
      Matrix A(kRowA, kColA);
      initMatrix(A);
      const int kRowB = kRowA;
      Matrix B(kRowB, kColB);
      initMatrix(B);
  
      for (int row_stride_c = kColA; row_stride_c < 3 * kColA; ++row_stride_c) {
        for (int col_stride_c = kColB; col_stride_c < 3 * kColB; ++col_stride_c) {
          Matrix C(row_stride_c, col_stride_c);
          C.setOnes();
  
          Matrix C_plus = C;
          Matrix C_minus = C;
          Matrix C_assign = C;
  
          Matrix C_plus_ref = C;
          Matrix C_minus_ref = C;
          Matrix C_assign_ref = C;
          for (int start_row_c = 0; start_row_c + kColA < row_stride_c;
               ++start_row_c) {
            for (int start_col_c = 0; start_col_c + kColB < col_stride_c;
                 ++start_col_c) {
              C_plus_ref.block(start_row_c, start_col_c, kColA, kColB) +=
                  A.transpose() * B;
  
              FunctorTy<kRowA, kColA, kRowB, kColB, 1, kDimType>()(
                  A.data(), kRowA, kColA, B.data(), kRowB, kColB, C_plus.data(),
                  start_row_c, start_col_c, row_stride_c, col_stride_c);
  
              EXPECT_NEAR((C_plus_ref - C_plus).norm(), 0.0, kTolerance)
                  << "C += A' * B \n"
                  << "row_stride_c : " << row_stride_c << "\n"
                  << "col_stride_c : " << col_stride_c << "\n"
                  << "start_row_c  : " << start_row_c << "\n"
                  << "start_col_c  : " << start_col_c << "\n"
                  << "Cref : \n"
                  << C_plus_ref << "\n"
                  << "C: \n"
                  << C_plus;
  
              C_minus_ref.block(start_row_c, start_col_c, kColA, kColB) -=
                  A.transpose() * B;
  
              FunctorTy<kRowA, kColA, kRowB, kColB, -1, kDimType>()(
                  A.data(), kRowA, kColA, B.data(), kRowB, kColB, C_minus.data(),
                  start_row_c, start_col_c, row_stride_c, col_stride_c);
  
              EXPECT_NEAR((C_minus_ref - C_minus).norm(), 0.0, kTolerance)
                  << "C -= A' * B \n"
                  << "row_stride_c : " << row_stride_c << "\n"
                  << "col_stride_c : " << col_stride_c << "\n"
                  << "start_row_c  : " << start_row_c << "\n"
                  << "start_col_c  : " << start_col_c << "\n"
                  << "Cref : \n"
                  << C_minus_ref << "\n"
                  << "C: \n"
                  << C_minus;
  
              C_assign_ref.block(start_row_c, start_col_c, kColA, kColB) =
                  A.transpose() * B;
  
              FunctorTy<kRowA, kColA, kRowB, kColB, 0, kDimType>()(
                  A.data(), kRowA, kColA, B.data(), kRowB, kColB, C_assign.data(),
                  start_row_c, start_col_c, row_stride_c, col_stride_c);
  
              EXPECT_NEAR((C_assign_ref - C_assign).norm(), 0.0, kTolerance)
                  << "C = A' * B \n"
                  << "row_stride_c : " << row_stride_c << "\n"
                  << "col_stride_c : " << col_stride_c << "\n"
                  << "start_row_c  : " << start_row_c << "\n"
                  << "start_col_c  : " << start_col_c << "\n"
                  << "Cref : \n"
                  << C_assign_ref << "\n"
                  << "C: \n"
                  << C_assign;
            }
          }
        }
      }
    }
  };
  
  TEST(BLAS, MatrixMatrixMultiply_5_3_7) {
    TestMatrixFunctions<5, 3, 7, DimType::Static, MatrixMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiply_5_3_7_Dynamic) {
    TestMatrixFunctions<5, 3, 7, DimType::Dynamic, MatrixMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiply_1_1_1) {
    TestMatrixFunctions<1, 1, 1, DimType::Static, MatrixMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiply_1_1_1_Dynamic) {
    TestMatrixFunctions<1, 1, 1, DimType::Dynamic, MatrixMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiply_9_9_9) {
    TestMatrixFunctions<9, 9, 9, DimType::Static, MatrixMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiply_9_9_9_Dynamic) {
    TestMatrixFunctions<9, 9, 9, DimType::Dynamic, MatrixMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiplyNaive_5_3_7) {
    TestMatrixFunctions<5, 3, 7, DimType::Static,
                        MatrixMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiplyNaive_5_3_7_Dynamic) {
    TestMatrixFunctions<5, 3, 7, DimType::Dynamic,
                        MatrixMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiplyNaive_1_1_1) {
    TestMatrixFunctions<1, 1, 1, DimType::Static,
                        MatrixMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiplyNaive_1_1_1_Dynamic) {
    TestMatrixFunctions<1, 1, 1, DimType::Dynamic,
                        MatrixMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiplyNaive_9_9_9) {
    TestMatrixFunctions<9, 9, 9, DimType::Static,
                        MatrixMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixMatrixMultiplyNaive_9_9_9_Dynamic) {
    TestMatrixFunctions<9, 9, 9, DimType::Dynamic,
                        MatrixMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiply_5_3_7) {
    TestMatrixTransposeFunctions<5, 3, 7, DimType::Static,
                                 MatrixTransposeMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiply_5_3_7_Dynamic) {
    TestMatrixTransposeFunctions<5, 3, 7, DimType::Dynamic,
                                 MatrixTransposeMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiply_1_1_1) {
    TestMatrixTransposeFunctions<1, 1, 1, DimType::Static,
                                 MatrixTransposeMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiply_1_1_1_Dynamic) {
    TestMatrixTransposeFunctions<1, 1, 1, DimType::Dynamic,
                                 MatrixTransposeMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiply_9_9_9) {
    TestMatrixTransposeFunctions<9, 9, 9, DimType::Static,
                                 MatrixTransposeMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiply_9_9_9_Dynamic) {
    TestMatrixTransposeFunctions<9, 9, 9, DimType::Dynamic,
                                 MatrixTransposeMatrixMultiplyTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiplyNaive_5_3_7) {
    TestMatrixTransposeFunctions<5, 3, 7, DimType::Static,
                                 MatrixTransposeMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiplyNaive_5_3_7_Dynamic) {
    TestMatrixTransposeFunctions<5, 3, 7, DimType::Dynamic,
                                 MatrixTransposeMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiplyNaive_1_1_1) {
    TestMatrixTransposeFunctions<1, 1, 1, DimType::Static,
                                 MatrixTransposeMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiplyNaive_1_1_1_Dynamic) {
    TestMatrixTransposeFunctions<1, 1, 1, DimType::Dynamic,
                                 MatrixTransposeMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiplyNaive_9_9_9) {
    TestMatrixTransposeFunctions<9, 9, 9, DimType::Static,
                                 MatrixTransposeMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixTransposeMatrixMultiplyNaive_9_9_9_Dynamic) {
    TestMatrixTransposeFunctions<9, 9, 9, DimType::Dynamic,
                                 MatrixTransposeMatrixMultiplyNaiveTy>()();
  }
  
  TEST(BLAS, MatrixVectorMultiply) {
    for (int num_rows_a = 1; num_rows_a < 10; ++num_rows_a) {
      for (int num_cols_a = 1; num_cols_a < 10; ++num_cols_a) {
        Matrix A(num_rows_a, num_cols_a);
        A.setOnes();
  
        Vector b(num_cols_a);
        b.setOnes();
  
        Vector c(num_rows_a);
        c.setOnes();
  
        Vector c_plus = c;
        Vector c_minus = c;
        Vector c_assign = c;
  
        Vector c_plus_ref = c;
        Vector c_minus_ref = c;
        Vector c_assign_ref = c;
  
        // clang-format off
        c_plus_ref += A * b;
        MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
            A.data(), num_rows_a, num_cols_a,
            b.data(),
            c_plus.data());
        EXPECT_NEAR((c_plus_ref - c_plus).norm(), 0.0, kTolerance)
            << "c += A * b \n"
            << "c_ref : \n" << c_plus_ref << "\n"
            << "c: \n" << c_plus;
  
        c_minus_ref -= A * b;
        MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, -1>(
            A.data(), num_rows_a, num_cols_a,
            b.data(),
            c_minus.data());
        EXPECT_NEAR((c_minus_ref - c_minus).norm(), 0.0, kTolerance)
            << "c -= A * b \n"
            << "c_ref : \n" << c_minus_ref << "\n"
            << "c: \n" << c_minus;
  
        c_assign_ref = A * b;
        MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 0>(
            A.data(), num_rows_a, num_cols_a,
            b.data(),
            c_assign.data());
        EXPECT_NEAR((c_assign_ref - c_assign).norm(), 0.0, kTolerance)
            << "c = A * b \n"
            << "c_ref : \n" << c_assign_ref << "\n"
            << "c: \n" << c_assign;
        // clang-format on
      }
    }
  }
  
  TEST(BLAS, MatrixTransposeVectorMultiply) {
    for (int num_rows_a = 1; num_rows_a < 10; ++num_rows_a) {
      for (int num_cols_a = 1; num_cols_a < 10; ++num_cols_a) {
        Matrix A(num_rows_a, num_cols_a);
        A.setRandom();
  
        Vector b(num_rows_a);
        b.setRandom();
  
        Vector c(num_cols_a);
        c.setOnes();
  
        Vector c_plus = c;
        Vector c_minus = c;
        Vector c_assign = c;
  
        Vector c_plus_ref = c;
        Vector c_minus_ref = c;
        Vector c_assign_ref = c;
  
        // clang-format off
        c_plus_ref += A.transpose() * b;
        MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
            A.data(), num_rows_a, num_cols_a,
            b.data(),
            c_plus.data());
        EXPECT_NEAR((c_plus_ref - c_plus).norm(), 0.0, kTolerance)
            << "c += A' * b \n"
            << "c_ref : \n" << c_plus_ref << "\n"
            << "c: \n" << c_plus;
  
        c_minus_ref -= A.transpose() * b;
        MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, -1>(
            A.data(), num_rows_a, num_cols_a,
            b.data(),
            c_minus.data());
        EXPECT_NEAR((c_minus_ref - c_minus).norm(), 0.0, kTolerance)
            << "c -= A' * b \n"
            << "c_ref : \n" << c_minus_ref << "\n"
            << "c: \n" << c_minus;
  
        c_assign_ref = A.transpose() * b;
        MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 0>(
            A.data(), num_rows_a, num_cols_a,
            b.data(),
            c_assign.data());
        EXPECT_NEAR((c_assign_ref - c_assign).norm(), 0.0, kTolerance)
            << "c = A' * b \n"
            << "c_ref : \n" << c_assign_ref << "\n"
            << "c: \n" << c_assign;
        // clang-format on
      }
    }
  }
  
  } // namespace internal
  } // namespace ceres
  