/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file SparseEigen.h
 *
 * @brief Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)
 *
 * @date Aug 2019
 * @author Mandy Xie
 * @author Fan Jiang
 * @author Gerry Chen
 * @author Frank Dellaert
 */

#pragma once

#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>

#include <Eigen/Sparse>

namespace gtsam {

/// Eigen-format sparse matrix.  Note: ColMajor is ~20% faster since
/// InnerIndices must be sorted
typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseEigen;

/// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
SparseEigen sparseJacobianEigen(
    const GaussianFactorGraph &gfg, const Ordering &ordering) {
  gttic_(SparseEigen_sparseJacobianEigen);
  // intermediate `entries` vector is kind of unavoidable due to how expensive
  // factor->rows() is, which prevents us from populating SparseEigen directly.
  // Triplet is about 11% faster than boost tuple.
  std::vector<Eigen::Triplet<double>> entries;
  entries.reserve(60 * gfg.size());
  size_t nrows, ncols;
  gfg.sparseJacobianInPlace(entries, ordering, nrows, ncols);
  // declare sparse matrix
  SparseEigen Ab(nrows, ncols);
  // See Eigen::set_from_triplets.  This is about 5% faster.
  // pass 1: count the nnz per inner-vector
  std::vector<int> nnz(ncols, 0);
  for (const auto &entry : entries) nnz[entry.col()]++;
  // pass 2: insert the elements
  Ab.reserve(nnz);
  for (const auto &entry : entries)
    Ab.insert(entry.row(), entry.col()) = entry.value();
  return Ab;
}

SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
  gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering);
  return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
}

}  // namespace gtsam