// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2019 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.
//
// Authors: sameeragarwal@google.com (Sameer Agarwal)

#include <memory>

#include "Eigen/Dense"
#include "benchmark/benchmark.h"
#include "ceres/block_random_access_dense_matrix.h"
#include "ceres/block_sparse_matrix.h"
#include "ceres/block_structure.h"
#include "ceres/random.h"
#include "ceres/schur_eliminator.h"

namespace ceres::internal {

constexpr int kRowBlockSize = 2;
constexpr int kEBlockSize = 3;
constexpr int kFBlockSize = 6;

class BenchmarkData {
 public:
  explicit BenchmarkData(const int num_e_blocks) {
    auto* bs = new CompressedRowBlockStructure;
    bs->cols.resize(num_e_blocks + 1);
    int col_pos = 0;
    for (int i = 0; i < num_e_blocks; ++i) {
      bs->cols[i].position = col_pos;
      bs->cols[i].size = kEBlockSize;
      col_pos += kEBlockSize;
    }
    bs->cols.back().position = col_pos;
    bs->cols.back().size = kFBlockSize;

    bs->rows.resize(2 * num_e_blocks);
    int row_pos = 0;
    int cell_pos = 0;
    for (int i = 0; i < num_e_blocks; ++i) {
      {
        auto& row = bs->rows[2 * i];
        row.block.position = row_pos;
        row.block.size = kRowBlockSize;
        row_pos += kRowBlockSize;
        auto& cells = row.cells;
        cells.resize(2);
        cells[0].block_id = i;
        cells[0].position = cell_pos;
        cell_pos += kRowBlockSize * kEBlockSize;
        cells[1].block_id = num_e_blocks;
        cells[1].position = cell_pos;
        cell_pos += kRowBlockSize * kFBlockSize;
      }
      {
        auto& row = bs->rows[2 * i + 1];
        row.block.position = row_pos;
        row.block.size = kRowBlockSize;
        row_pos += kRowBlockSize;
        auto& cells = row.cells;
        cells.resize(1);
        cells[0].block_id = i;
        cells[0].position = cell_pos;
        cell_pos += kRowBlockSize * kEBlockSize;
      }
    }

    matrix_ = std::make_unique<BlockSparseMatrix>(bs);
    double* values = matrix_->mutable_values();
    for (int i = 0; i < matrix_->num_nonzeros(); ++i) {
      values[i] = RandNormal();
    }

    b_.resize(matrix_->num_rows());
    b_.setRandom();

    std::vector<int> blocks(1, kFBlockSize);
    lhs_ = std::make_unique<BlockRandomAccessDenseMatrix>(blocks);
    diagonal_.resize(matrix_->num_cols());
    diagonal_.setOnes();
    rhs_.resize(kFBlockSize);

    y_.resize(num_e_blocks * kEBlockSize);
    y_.setZero();
    z_.resize(kFBlockSize);
    z_.setOnes();
  }

  const BlockSparseMatrix& matrix() const { return *matrix_; }
  const Vector& b() const { return b_; }
  const Vector& diagonal() const { return diagonal_; }
  BlockRandomAccessDenseMatrix* mutable_lhs() { return lhs_.get(); }
  Vector* mutable_rhs() { return &rhs_; }
  Vector* mutable_y() { return &y_; }
  Vector* mutable_z() { return &z_; }

 private:
  std::unique_ptr<BlockSparseMatrix> matrix_;
  Vector b_;
  std::unique_ptr<BlockRandomAccessDenseMatrix> lhs_;
  Vector rhs_;
  Vector diagonal_;
  Vector z_;
  Vector y_;
};

static void BM_SchurEliminatorEliminate(benchmark::State& state) {
  const int num_e_blocks = state.range(0);
  BenchmarkData data(num_e_blocks);

  ContextImpl context;
  LinearSolver::Options linear_solver_options;
  linear_solver_options.e_block_size = kEBlockSize;
  linear_solver_options.row_block_size = kRowBlockSize;
  linear_solver_options.f_block_size = kFBlockSize;
  linear_solver_options.context = &context;
  std::unique_ptr<SchurEliminatorBase> eliminator(
      SchurEliminatorBase::Create(linear_solver_options));

  eliminator->Init(num_e_blocks, true, data.matrix().block_structure());
  for (auto _ : state) {
    eliminator->Eliminate(BlockSparseMatrixData(data.matrix()),
                          data.b().data(),
                          data.diagonal().data(),
                          data.mutable_lhs(),
                          data.mutable_rhs()->data());
  }
}

static void BM_SchurEliminatorBackSubstitute(benchmark::State& state) {
  const int num_e_blocks = state.range(0);
  BenchmarkData data(num_e_blocks);

  ContextImpl context;
  LinearSolver::Options linear_solver_options;
  linear_solver_options.e_block_size = kEBlockSize;
  linear_solver_options.row_block_size = kRowBlockSize;
  linear_solver_options.f_block_size = kFBlockSize;
  linear_solver_options.context = &context;
  std::unique_ptr<SchurEliminatorBase> eliminator(
      SchurEliminatorBase::Create(linear_solver_options));

  eliminator->Init(num_e_blocks, true, data.matrix().block_structure());
  eliminator->Eliminate(BlockSparseMatrixData(data.matrix()),
                        data.b().data(),
                        data.diagonal().data(),
                        data.mutable_lhs(),
                        data.mutable_rhs()->data());
  for (auto _ : state) {
    eliminator->BackSubstitute(BlockSparseMatrixData(data.matrix()),
                               data.b().data(),
                               data.diagonal().data(),
                               data.mutable_z()->data(),
                               data.mutable_y()->data());
  }
}

static void BM_SchurEliminatorForOneFBlockEliminate(benchmark::State& state) {
  const int num_e_blocks = state.range(0);
  BenchmarkData data(num_e_blocks);
  SchurEliminatorForOneFBlock<2, 3, 6> eliminator;
  eliminator.Init(num_e_blocks, true, data.matrix().block_structure());
  for (auto _ : state) {
    eliminator.Eliminate(BlockSparseMatrixData(data.matrix()),
                         data.b().data(),
                         data.diagonal().data(),
                         data.mutable_lhs(),
                         data.mutable_rhs()->data());
  }
}

static void BM_SchurEliminatorForOneFBlockBackSubstitute(
    benchmark::State& state) {
  const int num_e_blocks = state.range(0);
  BenchmarkData data(num_e_blocks);
  SchurEliminatorForOneFBlock<2, 3, 6> eliminator;
  eliminator.Init(num_e_blocks, true, data.matrix().block_structure());
  eliminator.Eliminate(BlockSparseMatrixData(data.matrix()),
                       data.b().data(),
                       data.diagonal().data(),
                       data.mutable_lhs(),
                       data.mutable_rhs()->data());
  for (auto _ : state) {
    eliminator.BackSubstitute(BlockSparseMatrixData(data.matrix()),
                              data.b().data(),
                              data.diagonal().data(),
                              data.mutable_z()->data(),
                              data.mutable_y()->data());
  }
}

BENCHMARK(BM_SchurEliminatorEliminate)->Range(10, 10000);
BENCHMARK(BM_SchurEliminatorForOneFBlockEliminate)->Range(10, 10000);
BENCHMARK(BM_SchurEliminatorBackSubstitute)->Range(10, 10000);
BENCHMARK(BM_SchurEliminatorForOneFBlockBackSubstitute)->Range(10, 10000);

}  // namespace ceres::internal

BENCHMARK_MAIN();