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

#include "ceres/triplet_sparse_matrix.h"

#include <algorithm>

#include <cstddef>

#include "ceres/internal/eigen.h"

#include "ceres/internal/port.h"

#include "ceres/internal/scoped_ptr.h"

#include "ceres/random.h"

#include "ceres/types.h"

#include "glog/logging.h"

namespace ceres {

namespace internal {

TripletSparseMatrix::TripletSparseMatrix()

    : num_rows_(0),

      num_cols_(0),

      max_num_nonzeros_(0),

      num_nonzeros_(0),

      rows_(NULL),

      cols_(NULL),

      values_(NULL) {}

TripletSparseMatrix::~TripletSparseMatrix() {}

TripletSparseMatrix::TripletSparseMatrix(int num_rows,

                                         int num_cols,

                                         int max_num_nonzeros)

    : num_rows_(num_rows),

      num_cols_(num_cols),

      max_num_nonzeros_(max_num_nonzeros),

      num_nonzeros_(0),

      rows_(NULL),

      cols_(NULL),

      values_(NULL) {

  // All the sizes should at least be zero

  CHECK_GE(num_rows, 0);

  CHECK_GE(num_cols, 0);

  CHECK_GE(max_num_nonzeros, 0);

  AllocateMemory();

}

TripletSparseMatrix::TripletSparseMatrix(const int num_rows,

                                         const int num_cols,

                                         const std::vector<int>& rows,

                                         const std::vector<int>& cols,

                                         const std::vector<double>& values)

    : num_rows_(num_rows),

      num_cols_(num_cols),

      max_num_nonzeros_(values.size()),

      num_nonzeros_(values.size()),

      rows_(NULL),

      cols_(NULL),

      values_(NULL) {

  // All the sizes should at least be zero

  CHECK_GE(num_rows, 0);

  CHECK_GE(num_cols, 0);

  CHECK_EQ(rows.size(), cols.size());

  CHECK_EQ(rows.size(), values.size());

  AllocateMemory();

  std::copy(rows.begin(), rows.end(), rows_.get());

  std::copy(cols.begin(), cols.end(), cols_.get());

  std::copy(values.begin(), values.end(), values_.get());

}

TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig)

    : SparseMatrix(),

      num_rows_(orig.num_rows_),

      num_cols_(orig.num_cols_),

      max_num_nonzeros_(orig.max_num_nonzeros_),

      num_nonzeros_(orig.num_nonzeros_),

      rows_(NULL),

      cols_(NULL),

      values_(NULL) {

  AllocateMemory();

  CopyData(orig);

}

TripletSparseMatrix& TripletSparseMatrix::operator=(

    const TripletSparseMatrix& rhs) {

  num_rows_ = rhs.num_rows_;

  num_cols_ = rhs.num_cols_;

  num_nonzeros_ = rhs.num_nonzeros_;

  max_num_nonzeros_ = rhs.max_num_nonzeros_;

  AllocateMemory();

  CopyData(rhs);

  return *this;

}

bool TripletSparseMatrix::AllTripletsWithinBounds() const {

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

    if ((rows_[i] < 0) || (rows_[i] >= num_rows_) ||

        (cols_[i] < 0) || (cols_[i] >= num_cols_))

      return false;

  }

  return true;

}

void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) {

  CHECK_LE(num_nonzeros_, new_max_num_nonzeros)

      << "Reallocation will cause data loss";

  // Nothing to do if we have enough space already.

  if (new_max_num_nonzeros <= max_num_nonzeros_)

    return;

  int* new_rows = new int[new_max_num_nonzeros];

  int* new_cols = new int[new_max_num_nonzeros];

  double* new_values = new double[new_max_num_nonzeros];

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

    new_rows[i] = rows_[i];

    new_cols[i] = cols_[i];

    new_values[i] = values_[i];

  }

  rows_.reset(new_rows);

  cols_.reset(new_cols);

  values_.reset(new_values);

  max_num_nonzeros_ = new_max_num_nonzeros;

}

void TripletSparseMatrix::SetZero() {

  std::fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0);

  num_nonzeros_ = 0;

}

void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) {

  CHECK_GE(num_nonzeros, 0);

  CHECK_LE(num_nonzeros, max_num_nonzeros_);

  num_nonzeros_ = num_nonzeros;

}

void TripletSparseMatrix::AllocateMemory() {

  rows_.reset(new int[max_num_nonzeros_]);

  cols_.reset(new int[max_num_nonzeros_]);

  values_.reset(new double[max_num_nonzeros_]);

}

void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) {

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

    rows_[i] = orig.rows_[i];

    cols_[i] = orig.cols_[i];

    values_[i] = orig.values_[i];

  }

}

void TripletSparseMatrix::RightMultiply(const double* x,  double* y) const {

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

    y[rows_[i]] += values_[i]*x[cols_[i]];

  }

}

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

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

    y[cols_[i]] += values_[i]*x[rows_[i]];

  }

}

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

  CHECK_NOTNULL(x);

  VectorRef(x, num_cols_).setZero();

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

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

  }

}

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

  CHECK_NOTNULL(scale);

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

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

  }

}

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

  dense_matrix->resize(num_rows_, num_cols_);

  dense_matrix->setZero();

  Matrix& m = *dense_matrix;

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

    m(rows_[i], cols_[i]) += values_[i];

  }

}

void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) {

  CHECK_EQ(B.num_cols(), num_cols_);

  Reserve(num_nonzeros_ + B.num_nonzeros_);

  for (int i = 0; i < B.num_nonzeros_; ++i) {

    rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_;

    cols_.get()[num_nonzeros_] = B.cols()[i];

    values_.get()[num_nonzeros_++] = B.values()[i];

  }

  num_rows_ = num_rows_ + B.num_rows();

}

void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) {

  CHECK_EQ(B.num_rows(), num_rows_);

  Reserve(num_nonzeros_ + B.num_nonzeros_);

  for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) {

    rows_.get()[num_nonzeros_] = B.rows()[i];

    cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_;

    values_.get()[num_nonzeros_] = B.values()[i];

  }

  num_cols_ = num_cols_ + B.num_cols();

}

void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) {

  if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) {

    num_rows_  = new_num_rows;

    num_cols_ = new_num_cols;

    return;

  }

  num_rows_ = new_num_rows;

  num_cols_ = new_num_cols;

  int* r_ptr = rows_.get();

  int* c_ptr = cols_.get();

  double* v_ptr = values_.get();

  int dropped_terms = 0;

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

    if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) {

      if (dropped_terms) {

        r_ptr[i-dropped_terms] = r_ptr[i];

        c_ptr[i-dropped_terms] = c_ptr[i];

        v_ptr[i-dropped_terms] = v_ptr[i];

      }

    } else {

      ++dropped_terms;

    }

  }

  num_nonzeros_ -= dropped_terms;

}

TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix(

    const double* values, int num_rows) {

  TripletSparseMatrix* m =

      new TripletSparseMatrix(num_rows, num_rows, num_rows);

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

    m->mutable_rows()[i] = i;

    m->mutable_cols()[i] = i;

    m->mutable_values()[i] = values[i];

  }

  m->set_num_nonzeros(num_rows);

  return m;

}

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

  CHECK_NOTNULL(file);

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

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

  }

}

TripletSparseMatrix* TripletSparseMatrix::CreateRandomMatrix(

    const TripletSparseMatrix::RandomMatrixOptions& options) {

  CHECK_GT(options.num_rows, 0);

  CHECK_GT(options.num_cols, 0);

  CHECK_GT(options.density, 0.0);

  CHECK_LE(options.density, 1.0);

  std::vector<int> rows;

  std::vector<int> cols;

  std::vector<double> values;

  while (rows.empty()) {

    rows.clear();

    cols.clear();

    values.clear();

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

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

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

          rows.push_back(r);

          cols.push_back(c);

          values.push_back(RandNormal());

        }

      }

    }

  }

  return new TripletSparseMatrix(

      options.num_rows, options.num_cols, rows, cols, values);

}

}  // namespace internal

}  // namespace ceres