// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2017 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: sameeragarwal@google.com (Sameer Agarwal)

#include "ceres/compressed_row_sparse_matrix.h"

#include <algorithm>

#include <numeric>

#include <vector>

#include "ceres/crs_matrix.h"

#include "ceres/internal/port.h"

#include "ceres/random.h"

#include "ceres/triplet_sparse_matrix.h"

#include "glog/logging.h"

namespace ceres {

namespace internal {

using std::vector;

namespace {

// Helper functor used by the constructor for reordering the contents

// of a TripletSparseMatrix. This comparator assumes thay there are no

// duplicates in the pair of arrays rows and cols, i.e., there is no

// indices i and j (not equal to each other) s.t.

//

//  rows[i] == rows[j] && cols[i] == cols[j]

//

// If this is the case, this functor will not be a StrictWeakOrdering.

struct RowColLessThan {

  RowColLessThan(const int* rows, const int* cols) : rows(rows), cols(cols) {}

  bool operator()(const int x, const int y) const {

    if (rows[x] == rows[y]) {

      return (cols[x] < cols[y]);

    }

    return (rows[x] < rows[y]);

  }

  const int* rows;

  const int* cols;

};

void TransposeForCompressedRowSparseStructure(const int num_rows,

                                              const int num_cols,

                                              const int num_nonzeros,

                                              const int* rows,

                                              const int* cols,

                                              const double* values,

                                              int* transpose_rows,

                                              int* transpose_cols,

                                              double* transpose_values) {

  // Explicitly zero out transpose_rows.

  std::fill(transpose_rows, transpose_rows + num_cols + 1, 0);

  // Count the number of entries in each column of the original matrix

  // and assign to transpose_rows[col + 1].

  for (int idx = 0; idx < num_nonzeros; ++idx) {

    ++transpose_rows[cols[idx] + 1];

  }

  // Compute the starting position for each row in the transpose by

  // computing the cumulative sum of the entries of transpose_rows.

  for (int i = 1; i < num_cols + 1; ++i) {

    transpose_rows[i] += transpose_rows[i - 1];

  }

  // Populate transpose_cols and (optionally) transpose_values by

  // walking the entries of the source matrices. For each entry that

  // is added, the value of transpose_row is incremented allowing us

  // to keep track of where the next entry for that row should go.

  //

  // As a result transpose_row is shifted to the left by one entry.

  for (int r = 0; r < num_rows; ++r) {

    for (int idx = rows[r]; idx < rows[r + 1]; ++idx) {

      const int c = cols[idx];

      const int transpose_idx = transpose_rows[c]++;

      transpose_cols[transpose_idx] = r;

      if (values != NULL && transpose_values != NULL) {

        transpose_values[transpose_idx] = values[idx];

      }

    }

  }

  // This loop undoes the left shift to transpose_rows introduced by

  // the previous loop.

  for (int i = num_cols - 1; i > 0; --i) {

    transpose_rows[i] = transpose_rows[i - 1];

  }

  transpose_rows[0] = 0;

}

void AddRandomBlock(const int num_rows,

                    const int num_cols,

                    const int row_block_begin,

                    const int col_block_begin,

                    std::vector<int>* rows,

                    std::vector<int>* cols,

                    std::vector<double>* values) {

  for (int r = 0; r < num_rows; ++r) {

    for (int c = 0; c < num_cols; ++c) {

      rows->push_back(row_block_begin + r);

      cols->push_back(col_block_begin + c);

      values->push_back(RandNormal());

    }

  }

}

}  // namespace

// This constructor gives you a semi-initialized CompressedRowSparseMatrix.

CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows,

                                                     int num_cols,

                                                     int max_num_nonzeros) {

  num_rows_ = num_rows;

  num_cols_ = num_cols;

  storage_type_ = UNSYMMETRIC;

  rows_.resize(num_rows + 1, 0);

  cols_.resize(max_num_nonzeros, 0);

  values_.resize(max_num_nonzeros, 0.0);

  VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_

          << " max_num_nonzeros: " << cols_.size() << ". Allocating "

          << (num_rows_ + 1) * sizeof(int) +     // NOLINT

                 cols_.size() * sizeof(int) +    // NOLINT

                 cols_.size() * sizeof(double);  // NOLINT

}

CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix(

    const TripletSparseMatrix& input) {

  return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, false);

}

CompressedRowSparseMatrix*

CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(

    const TripletSparseMatrix& input) {

  return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, true);

}

CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix(

    const TripletSparseMatrix& input, bool transpose) {

  int num_rows = input.num_rows();

  int num_cols = input.num_cols();

  const int* rows = input.rows();

  const int* cols = input.cols();

  const double* values = input.values();

  if (transpose) {

    std::swap(num_rows, num_cols);

    std::swap(rows, cols);

  }

  // index is the list of indices into the TripletSparseMatrix input.

  vector<int> index(input.num_nonzeros(), 0);

  for (int i = 0; i < input.num_nonzeros(); ++i) {

    index[i] = i;

  }

  // Sort index such that the entries of m are ordered by row and ties

  // are broken by column.

  std::sort(index.begin(), index.end(), RowColLessThan(rows, cols));

  VLOG(1) << "# of rows: " << num_rows << " # of columns: " << num_cols

          << " num_nonzeros: " << input.num_nonzeros() << ". Allocating "

          << ((num_rows + 1) * sizeof(int) +           // NOLINT

              input.num_nonzeros() * sizeof(int) +     // NOLINT

              input.num_nonzeros() * sizeof(double));  // NOLINT

  CompressedRowSparseMatrix* output =

      new CompressedRowSparseMatrix(num_rows, num_cols, input.num_nonzeros());

  if (num_rows == 0) {

    // No data to copy.

    return output;

  }

  // Copy the contents of the cols and values array in the order given

  // by index and count the number of entries in each row.

  int* output_rows = output->mutable_rows();

  int* output_cols = output->mutable_cols();

  double* output_values = output->mutable_values();

  output_rows[0] = 0;

  for (int i = 0; i < index.size(); ++i) {

    const int idx = index[i];

    ++output_rows[rows[idx] + 1];

    output_cols[i] = cols[idx];

    output_values[i] = values[idx];

  }

  // Find the cumulative sum of the row counts.

  for (int i = 1; i < num_rows + 1; ++i) {

    output_rows[i] += output_rows[i - 1];

  }

  CHECK_EQ(output->num_nonzeros(), input.num_nonzeros());

  return output;

}

CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal,

                                                     int num_rows) {

  CHECK_NOTNULL(diagonal);

  num_rows_ = num_rows;

  num_cols_ = num_rows;

  storage_type_ = UNSYMMETRIC;

  rows_.resize(num_rows + 1);

  cols_.resize(num_rows);

  values_.resize(num_rows);

  rows_[0] = 0;

  for (int i = 0; i < num_rows_; ++i) {

    cols_[i] = i;

    values_[i] = diagonal[i];

    rows_[i + 1] = i + 1;

  }

  CHECK_EQ(num_nonzeros(), num_rows);

}

CompressedRowSparseMatrix::~CompressedRowSparseMatrix() {}

void CompressedRowSparseMatrix::SetZero() {

  std::fill(values_.begin(), values_.end(), 0);

}

void CompressedRowSparseMatrix::RightMultiply(const double* x,

                                              double* y) const {

  CHECK_NOTNULL(x);

  CHECK_NOTNULL(y);

  for (int r = 0; r < num_rows_; ++r) {

    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {

      y[r] += values_[idx] * x[cols_[idx]];

    }

  }

}

void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const {

  CHECK_NOTNULL(x);

  CHECK_NOTNULL(y);

  for (int r = 0; r < num_rows_; ++r) {

    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {

      y[cols_[idx]] += values_[idx] * x[r];

    }

  }

}

void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const {

  CHECK_NOTNULL(x);

  std::fill(x, x + num_cols_, 0.0);

  for (int idx = 0; idx < rows_[num_rows_]; ++idx) {

    x[cols_[idx]] += values_[idx] * values_[idx];

  }

}

void CompressedRowSparseMatrix::ScaleColumns(const double* scale) {

  CHECK_NOTNULL(scale);

  for (int idx = 0; idx < rows_[num_rows_]; ++idx) {

    values_[idx] *= scale[cols_[idx]];

  }

}

void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {

  CHECK_NOTNULL(dense_matrix);

  dense_matrix->resize(num_rows_, num_cols_);

  dense_matrix->setZero();

  for (int r = 0; r < num_rows_; ++r) {

    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {

      (*dense_matrix)(r, cols_[idx]) = values_[idx];

    }

  }

}

void CompressedRowSparseMatrix::DeleteRows(int delta_rows) {

  CHECK_GE(delta_rows, 0);

  CHECK_LE(delta_rows, num_rows_);

  num_rows_ -= delta_rows;

  rows_.resize(num_rows_ + 1);

  // The rest of the code updates the block information. Immediately

  // return in case of no block information.

  if (row_blocks_.empty()) {

    return;

  }

  // Walk the list of row blocks until we reach the new number of rows

  // and the drop the rest of the row blocks.

  int num_row_blocks = 0;

  int num_rows = 0;

  while (num_row_blocks < row_blocks_.size() && num_rows < num_rows_) {

    num_rows += row_blocks_[num_row_blocks];

    ++num_row_blocks;

  }

  row_blocks_.resize(num_row_blocks);

}

void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) {

  CHECK_EQ(m.num_cols(), num_cols_);

  CHECK((row_blocks_.empty() && m.row_blocks().empty()) ||

        (!row_blocks_.empty() && !m.row_blocks().empty()))

      << "Cannot append a matrix with row blocks to one without and vice versa."

      << "This matrix has : " << row_blocks_.size() << " row blocks."

      << "The matrix being appended has: " << m.row_blocks().size()

      << " row blocks.";

  if (m.num_rows() == 0) {

    return;

  }

  if (cols_.size() < num_nonzeros() + m.num_nonzeros()) {

    cols_.resize(num_nonzeros() + m.num_nonzeros());

    values_.resize(num_nonzeros() + m.num_nonzeros());

  }

  // Copy the contents of m into this matrix.

  DCHECK_LT(num_nonzeros(), cols_.size());

  if (m.num_nonzeros() > 0) {

    std::copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]);

    std::copy(

        m.values(), m.values() + m.num_nonzeros(), &values_[num_nonzeros()]);

  }

  rows_.resize(num_rows_ + m.num_rows() + 1);

  // new_rows = [rows_, m.row() + rows_[num_rows_]]

  std::fill(rows_.begin() + num_rows_,

            rows_.begin() + num_rows_ + m.num_rows() + 1,

            rows_[num_rows_]);

  for (int r = 0; r < m.num_rows() + 1; ++r) {

    rows_[num_rows_ + r] += m.rows()[r];

  }

  num_rows_ += m.num_rows();

  // The rest of the code updates the block information. Immediately

  // return in case of no block information.

  if (row_blocks_.empty()) {

    return;

  }

  row_blocks_.insert(

      row_blocks_.end(), m.row_blocks().begin(), m.row_blocks().end());

}

void CompressedRowSparseMatrix::ToTextFile(FILE* file) const {

  CHECK_NOTNULL(file);

  for (int r = 0; r < num_rows_; ++r) {

    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {

      fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]);

    }

  }

}

void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const {

  matrix->num_rows = num_rows_;

  matrix->num_cols = num_cols_;

  matrix->rows = rows_;

  matrix->cols = cols_;

  matrix->values = values_;

  // Trim.

  matrix->rows.resize(matrix->num_rows + 1);

  matrix->cols.resize(matrix->rows[matrix->num_rows]);

  matrix->values.resize(matrix->rows[matrix->num_rows]);

}

void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) {

  CHECK_GE(num_nonzeros, 0);

  cols_.resize(num_nonzeros);

  values_.resize(num_nonzeros);

}

CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(

    const double* diagonal, const vector<int>& blocks) {

  int num_rows = 0;

  int num_nonzeros = 0;

  for (int i = 0; i < blocks.size(); ++i) {

    num_rows += blocks[i];

    num_nonzeros += blocks[i] * blocks[i];

  }

  CompressedRowSparseMatrix* matrix =

      new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros);

  int* rows = matrix->mutable_rows();

  int* cols = matrix->mutable_cols();

  double* values = matrix->mutable_values();

  std::fill(values, values + num_nonzeros, 0.0);

  int idx_cursor = 0;

  int col_cursor = 0;

  for (int i = 0; i < blocks.size(); ++i) {

    const int block_size = blocks[i];

    for (int r = 0; r < block_size; ++r) {

      *(rows++) = idx_cursor;

      values[idx_cursor + r] = diagonal[col_cursor + r];

      for (int c = 0; c < block_size; ++c, ++idx_cursor) {

        *(cols++) = col_cursor + c;

      }

    }

    col_cursor += block_size;

  }

  *rows = idx_cursor;

  *matrix->mutable_row_blocks() = blocks;

  *matrix->mutable_col_blocks() = blocks;

  CHECK_EQ(idx_cursor, num_nonzeros);

  CHECK_EQ(col_cursor, num_rows);

  return matrix;

}

CompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const {

  CompressedRowSparseMatrix* transpose =

      new CompressedRowSparseMatrix(num_cols_, num_rows_, num_nonzeros());

  switch (storage_type_) {

    case UNSYMMETRIC:

      transpose->set_storage_type(UNSYMMETRIC);

      break;

    case LOWER_TRIANGULAR:

      transpose->set_storage_type(UPPER_TRIANGULAR);

      break;

    case UPPER_TRIANGULAR:

      transpose->set_storage_type(LOWER_TRIANGULAR);

      break;

    default:

      LOG(FATAL) << "Unknown storage type: " << storage_type_;

  };

  TransposeForCompressedRowSparseStructure(num_rows(),

                                           num_cols(),

                                           num_nonzeros(),

                                           rows(),

                                           cols(),

                                           values(),

                                           transpose->mutable_rows(),

                                           transpose->mutable_cols(),

                                           transpose->mutable_values());

  // The rest of the code updates the block information. Immediately

  // return in case of no block information.

  if (row_blocks_.empty()) {

    return transpose;

  }

  *(transpose->mutable_row_blocks()) = col_blocks_;

  *(transpose->mutable_col_blocks()) = row_blocks_;

  return transpose;

}

CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateRandomMatrix(

    const CompressedRowSparseMatrix::RandomMatrixOptions& options) {

  CHECK_GT(options.num_row_blocks, 0);

  CHECK_GT(options.min_row_block_size, 0);

  CHECK_GT(options.max_row_block_size, 0);

  CHECK_LE(options.min_row_block_size, options.max_row_block_size);

  CHECK_GT(options.num_col_blocks, 0);

  CHECK_GT(options.min_col_block_size, 0);

  CHECK_GT(options.max_col_block_size, 0);

  CHECK_LE(options.min_col_block_size, options.max_col_block_size);

  CHECK_GT(options.block_density, 0.0);

  CHECK_LE(options.block_density, 1.0);

  vector<int> row_blocks;

  vector<int> col_blocks;

  // Generate the row block structure.

  for (int i = 0; i < options.num_row_blocks; ++i) {

    // Generate a random integer in [min_row_block_size, max_row_block_size]

    const int delta_block_size =

        Uniform(options.max_row_block_size - options.min_row_block_size);

    row_blocks.push_back(options.min_row_block_size + delta_block_size);

  }

  // Generate the col block structure.

  for (int i = 0; i < options.num_col_blocks; ++i) {

    // Generate a random integer in [min_col_block_size, max_col_block_size]

    const int delta_block_size =

        Uniform(options.max_col_block_size - options.min_col_block_size);

    col_blocks.push_back(options.min_col_block_size + delta_block_size);

  }

  vector<int> tsm_rows;

  vector<int> tsm_cols;

  vector<double> tsm_values;

  // For ease of construction, we are going to generate the

  // CompressedRowSparseMatrix by generating it as a

  // TripletSparseMatrix and then converting it to a

  // CompressedRowSparseMatrix.

  // It is possible that the random matrix is empty which is likely

  // not what the user wants, so do the matrix generation till we have

  // at least one non-zero entry.

  while (tsm_values.empty()) {

    tsm_rows.clear();

    tsm_cols.clear();

    tsm_values.clear();

    int row_block_begin = 0;

    for (int r = 0; r < options.num_row_blocks; ++r) {

      int col_block_begin = 0;

      for (int c = 0; c < options.num_col_blocks; ++c) {

        // Randomly determine if this block is present or not.

        if (RandDouble() <= options.block_density) {

          AddRandomBlock(row_blocks[r],

                         col_blocks[c],

                         row_block_begin,

                         col_block_begin,

                         &tsm_rows,

                         &tsm_cols,

                         &tsm_values);

        }

        col_block_begin += col_blocks[c];

      }

      row_block_begin += row_blocks[r];

    }

  }

  const int num_rows = std::accumulate(row_blocks.begin(), row_blocks.end(), 0);

  const int num_cols = std::accumulate(col_blocks.begin(), col_blocks.end(), 0);

  const bool kDoNotTranspose = false;

  CompressedRowSparseMatrix* matrix =

      CompressedRowSparseMatrix::FromTripletSparseMatrix(

          TripletSparseMatrix(

              num_rows, num_cols, tsm_rows, tsm_cols, tsm_values),

          kDoNotTranspose);

  (*matrix->mutable_row_blocks()) = row_blocks;

  (*matrix->mutable_col_blocks()) = col_blocks;

  matrix->set_storage_type(CompressedRowSparseMatrix::UNSYMMETRIC);

  return matrix;

}

}  // namespace internal

}  // namespace ceres