/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/config.h>  //MRPT_HAS_SUITESPARSE
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CSparseMatrixTemplate.h>
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/math/math_frwds.h>

#include <cstring>  // memcpy
#include <stdexcept>

// Include CSparse lib headers, either from the system or embedded:
#if MRPT_HAS_SUITESPARSE
#include "cs.h"
#else
extern "C"
{
#include <mrpt/3rdparty/CSparse/cs.h>
}
#endif

namespace mrpt::math
{
/** Used in mrpt::math::CSparseMatrix */
struct CExceptionNotDefPos : public std::runtime_error
{
  CExceptionNotDefPos(const char* s) : std::runtime_error(s) {}
};

/** A sparse matrix structure, wrapping T. Davis' CSparse library (part of
 *suitesparse)
 *  The type of the matrix entries is fixed to "double".
 *
 *  There are two formats for the non-zero entries in this matrix:
 *		- A "triplet" matrix: a set of (r,c)=val triplet entries.
 *		- A column-compressed sparse (CCS) matrix.
 *
 *  The latter is the "normal" format, which is expected by all mathematical
 *operations defined
 *   in this class. There're three ways of initializing and populating a sparse
 *matrix:
 *
 *   <ol>
 *    <li> <b>As a triplet (empty), then add entries, then compress:</b>
 *         \code
 *             CSparseMatrix  SM(100,100);
 *             SM.insert_entry(i,j, val);  // or
 *             SM.insert_submatrix(i,j, MAT); //  ...
 *             SM.compressFromTriplet();
 *         \endcode
 *    </li>
 *    <li> <b>As a triplet from a CSparseMatrixTemplate<double>:</b>
 *         \code
 *             CSparseMatrixTemplate<double>  data;
 *             data(row,col) = val;
 *             ...
 *             CSparseMatrix  SM(data);
 *         \endcode
 *    </li>
 *    <li> <b>From an existing dense matrix:</b>
 *         \code
 *             CMatrixDouble data(100,100); // or
 *             CMatrixFloat  data(100,100); // or
 *             CMatrixFixed<double,4,6>  data; // etc...
 *             CSparseMatrix  SM(data);
 *         \endcode
 *    </li>
 *
 *   </ol>
 *
 *  Due to its practical utility, there is a special inner class
 *CSparseMatrix::CholeskyDecomp to handle Cholesky-related methods and data.
 *
 * \note This class was initially adapted from "robotvision", by Hauke
 *Strasdat, Steven Lovegrove and Andrew J. Davison. See
 *http://www.openslam.org/robotvision.html
 * \note CSparse is maintained by Timothy Davis:
 *http://people.sc.fsu.edu/~jburkardt/c_src/csparse/csparse.html .
 * \note See also his book "Direct methods for sparse linear systems".
 *http://books.google.es/books?id=TvwiyF8vy3EC&pg=PA12&lpg=PA12&dq=cs_compress&source=bl&ots=od9uGJ793j&sig=Wa-fBk4sZkZv3Y0Op8FNH8PvCUs&hl=es&ei=UjA0TJf-EoSmsQay3aXPAw&sa=X&oi=book_result&ct=result&resnum=8&ved=0CEQQ6AEwBw#v=onepage&q&f=false
 *
 * \sa mrpt::math::MatrixBlockSparseCols, mrpt::math::CMatrixFixed,
 *mrpt::math::CMatrixDynamic, etc.
 * \ingroup mrpt_math_grp
 */
class CSparseMatrix
{
 private:
  cs sparse_matrix;

  /** Initialization from a dense matrix of any kind existing in MRPT. */
  template <class MATRIX>
  void construct_from_mrpt_mat(const MATRIX& C)
  {
    std::vector<int> row_list, col_list;  // Use "int" for convenience so we
    // can do a memcpy below...
    std::vector<double> content_list;
    const int nCol = C.cols();
    const int nRow = C.rows();
    for (int c = 0; c < nCol; ++c)
    {
      col_list.push_back(row_list.size());
      for (int r = 0; r < nRow; ++r)
        if (C(r, c) != 0)
        {
          row_list.push_back(r);
          content_list.push_back(C(r, c));
        }
    }
    col_list.push_back(row_list.size());

    sparse_matrix.m = nRow;
    sparse_matrix.n = nCol;
    sparse_matrix.nzmax = content_list.size();
    sparse_matrix.i = (int*)malloc(sizeof(int) * row_list.size());
    sparse_matrix.p = (int*)malloc(sizeof(int) * col_list.size());
    sparse_matrix.x = (double*)malloc(sizeof(double) * content_list.size());

    std::memcpy(sparse_matrix.i, &row_list[0], sizeof(row_list[0]) * row_list.size());
    std::memcpy(sparse_matrix.p, &col_list[0], sizeof(col_list[0]) * col_list.size());
    std::memcpy(sparse_matrix.x, &content_list[0], sizeof(content_list[0]) * content_list.size());

    sparse_matrix.nz = -1;  // <0 means "column compressed", ">=0" means triplet.
  }

  /** Initialization from a triplet "cs", which is first compressed */
  void construct_from_triplet(const cs& triplet);

  /** To be called by constructors only, assume previous pointers are trash
   * and overwrite them */
  void construct_from_existing_cs(const cs& sm);

  /** free buffers (deallocate the memory of the i,p,x buffers) */
  void internal_free_mem();

  /** Copy the data from an existing "cs" CSparse data structure */
  void copy(const cs* const sm);

  /** Fast copy the data from an existing "cs" CSparse data structure, copying
   * the pointers and leaving NULLs in the source structure. */
  void copy_fast(cs* const sm);

 public:
  /** @name Constructors, destructor & copy operations
    @{  */

  /** Create an initially empty sparse matrix, in the "triplet" form.
   *  Notice that you must call "compressFromTriplet" after populating the
   * matrix and before using the math operatons on this matrix.
   *  The initial size can be later on extended with insert_entry() or
   * setRowCount() & setColCount().
   * \sa insert_entry, setRowCount, setColCount
   */
  CSparseMatrix(size_t nRows = 0, size_t nCols = 0);

  /** A good way to initialize a sparse matrix from a list of non nullptr
   * elements.
   *  This constructor takes all the non-zero entries in "data" and builds a
   * column-compressed sparse representation.
   */
  template <typename T>
  inline CSparseMatrix(const CSparseMatrixTemplate<T>& data)
  {
    ASSERTMSG_(!data.empty(), "Input data must contain at least one non-zero element.");
    sparse_matrix.i = nullptr;  // This is to know they shouldn't be tried to free()
    sparse_matrix.p = nullptr;
    sparse_matrix.x = nullptr;

    // 1) Create triplet matrix
    CSparseMatrix triplet(data.rows(), data.cols());
    // 2) Put data in:
    for (auto it = data.begin(); it != data.end(); ++it)
      triplet.insert_entry_fast(it->first.first, it->first.second, it->second);

    // 3) Compress:
    construct_from_triplet(triplet.sparse_matrix);
  }

  // We can't do a simple "template <class ANYMATRIX>" since it would be tried
  // to match against "cs*"...

  /** Constructor from a dense matrix of any kind existing in MRPT, creating a
   * "column-compressed" sparse matrix. */
  template <typename T, size_t N, size_t M>
  inline explicit CSparseMatrix(const CMatrixFixed<T, N, M>& MAT)
  {
    construct_from_mrpt_mat(MAT);
  }

  /** Constructor from a dense matrix of any kind existing in MRPT, creating a
   * "column-compressed" sparse matrix. */
  template <typename T>
  inline explicit CSparseMatrix(const CMatrixDynamic<T>& MAT)
  {
    construct_from_mrpt_mat(MAT);
  }

  /** Copy constructor */
  CSparseMatrix(const CSparseMatrix& other);

  /** Copy constructor from an existing "cs" CSparse data structure */
  explicit CSparseMatrix(const cs* const sm);

  /** Destructor */
  virtual ~CSparseMatrix();

  /** Copy operator from another existing object */
  CSparseMatrix& operator=(const CSparseMatrix& other);

  /** Fast swap contents with another sparse matrix */
  void swap(CSparseMatrix& other);

  /** Erase all previous contents and leave the matrix as a "triplet" ROWS x
   * COLS matrix without any nonzero entry. */
  void clear(size_t nRows = 1, size_t nCols = 1);

  /** @}  */

  /** @name Math operations (the interesting stuff...)
    @{ */

  /** this = A+B */
  void add_AB(const CSparseMatrix& A, const CSparseMatrix& B);
  /** this = A*B */
  void multiply_AB(const CSparseMatrix& A, const CSparseMatrix& B);
  /** out_res = this * b */
  void matProductOf_Ab(
      const mrpt::math::CVectorDouble& b, mrpt::math::CVectorDouble& out_res) const;

  inline CSparseMatrix operator+(const CSparseMatrix& other) const
  {
    CSparseMatrix RES;
    RES.add_AB(*this, other);
    return RES;
  }
  inline CSparseMatrix operator*(const CSparseMatrix& other) const
  {
    CSparseMatrix RES;
    RES.multiply_AB(*this, other);
    return RES;
  }
  inline mrpt::math::CVectorDouble operator*(const mrpt::math::CVectorDouble& other) const
  {
    mrpt::math::CVectorDouble res;
    matProductOf_Ab(other, res);
    return res;
  }
  inline void operator+=(const CSparseMatrix& other) { this->add_AB(*this, other); }
  inline void operator*=(const CSparseMatrix& other) { this->multiply_AB(*this, other); }
  CSparseMatrix transpose() const;

  /** @}  */

  /** @ Access the matrix, get, set elements, etc.
    @{ */

  /** ONLY for TRIPLET matrices: insert a new non-zero entry in the matrix.
   *  This method cannot be used once the matrix is in column-compressed
   * form.
   *  The size of the matrix will be automatically extended if the indices
   * are out of the current limits.
   * \sa isTriplet, compressFromTriplet
   */
  void insert_entry(size_t row, size_t col, const double val);

  /** This was an optimized version, but is now equivalent to insert_entry()
   * due to the need to be compatible with unmodified CSparse system
   * libraries. */
  inline void insert_entry_fast(size_t row, size_t col, const double val)
  {
    insert_entry(row, col, val);
  }

  /** ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT
   * formats) at a given location of the sparse matrix.
   *  This method cannot be used once the matrix is in column-compressed
   * form.
   *  The size of the matrix will be automatically extended if the indices
   * are out of the current limits.
   *  Since this is inline, it can be very efficient for fixed-size MRPT
   * matrices.
   * \sa isTriplet, compressFromTriplet, insert_entry
   */
  template <class MATRIX>
  inline void insert_submatrix(size_t row, size_t col, const MATRIX& M)
  {
    if (!isTriplet())
      THROW_EXCEPTION(
          "insert_entry() is only available for sparse matrix in "
          "'triplet' format.");
    const size_t nR = M.rows();
    const size_t nC = M.cols();
    for (size_t r = 0; r < nR; r++)
      for (size_t c = 0; c < nC; c++) insert_entry_fast(row + r, col + c, M(r, c));
    // If needed, extend the size of the matrix:
    sparse_matrix.m = std::max(sparse_matrix.m, int(row + nR));
    sparse_matrix.n = std::max(sparse_matrix.n, int(col + nC));
  }

  /** ONLY for TRIPLET matrices: convert the matrix in a column-compressed
   * form.
   * \sa insert_entry
   */
  void compressFromTriplet();

  /** Return a dense representation of the sparse matrix.
   * \sa saveToTextFile_dense
   */
  void get_dense(CMatrixDouble& outMat) const;

  /** Static method to convert a "cs" structure into a dense representation of
   * the sparse matrix.
   */
  static void cs2dense(const cs& SM, CMatrixDouble& outMat);

  /** save as a dense matrix to a text file \return False on any error.
   */
  bool saveToTextFile_dense(const std::string& filName);

  /** Save sparse structure to a text file loadable from MATLAB (can be called
   * on triplet or CCS matrices).
   *
   *  The format of the text file is:
   *  \code
   *   NUM_ROWS   NUM_COLS   NUM_NON_ZERO_MAX
   *   row_1   col_1   value_1
   *   row_2   col_2   value_2
   *   ...
   *  \endcode
   *
   *  Instructions for loading from MATLAB in triplet form will be
   * automatically written to the output file as comments in th first lines:
   *
   *   \code
   *      D=load('file.txt');
   *      SM=spconvert(D(2:end,:));
   *        or, to always preserve the actual matrix size m x n:
   *      m=D(1,1); n=D(1,2); nzmax=D(1,3);
   *      Di=D(2:end,1); Dj=D(2:end,2); Ds=D(2:end,3);
   *      M=sparse(Di,Dj,Ds, m,n, nzmax);
   *   \endcode
   * \return False on any error.
   */
  [[nodiscard]] bool saveToTextFile_sparse(const std::string& filName);

  // Very basic, standard methods that MRPT methods expect for any matrix:
  inline size_t rows() const { return sparse_matrix.m; }
  inline size_t cols() const { return sparse_matrix.n; }
  /** Change the number of rows in the matrix (can't be lower than current
   * size) */
  inline void setRowCount(size_t nRows)
  {
    ASSERT_(nRows >= (size_t)sparse_matrix.m);
    sparse_matrix.m = nRows;
  }
  inline void setColCount(size_t nCols)
  {
    ASSERT_(nCols >= (size_t)sparse_matrix.n);
    sparse_matrix.n = nCols;
  }

  /** Returns true if this sparse matrix is in "triplet" form. \sa
   * isColumnCompressed */
  inline bool isTriplet() const
  {
    return sparse_matrix.nz >= 0;
  }  // <0 means "column compressed", ">=0" means triplet.

  /** Returns true if this sparse matrix is in "column compressed" form. \sa
   * isTriplet */
  inline bool isColumnCompressed() const
  {
    return sparse_matrix.nz < 0;
  }  // <0 means "column compressed", ">=0" means triplet.

  /** @} */

  /** @name Cholesky factorization
    @{  */

  /** Auxiliary class to hold the results of a Cholesky factorization of a
   *sparse matrix.
   *  This implementation does not allow updating/downdating.
   *
   *  Usage example:
   *   \code
   *     CSparseMatrix  SM(100,100);
   *     SM.insert_entry(i,j, val); ...
   *     SM.compressFromTriplet();
   *
   *     // Do Cholesky decomposition:
   *		CSparseMatrix::CholeskyDecomp  CD(SM);
   *     CD.get_inverse();
   *     ...
   *   \endcode
   *
   * \note Only the upper triangular part of the input matrix is accessed.
   * \note This class was initially adapted from "robotvision", by Hauke
   *Strasdat, Steven Lovegrove and Andrew J. Davison. See
   *http://www.openslam.org/robotvision.html
   * \note This class designed to be "uncopiable".
   * \sa The main class: CSparseMatrix
   */
  class CholeskyDecomp
  {
   private:
    css* m_symbolic_structure;
    csn* m_numeric_structure;
    /** A const reference to the original matrix used to build this
     * decomposition. */
    const CSparseMatrix* m_originalSM;

   public:
    /** Constructor from a square definite-positive sparse matrix A, which
     * can be use to solve Ax=b
     *   The actual Cholesky decomposition takes places in this
     * constructor.
     *  \note Only the upper triangular part of the matrix is accessed.
     *  \exception std::runtime_error On non-square input matrix.
     *  \exception mrpt::math::CExceptionNotDefPos On non-definite-positive
     * matrix as input.
     */
    CholeskyDecomp(const CSparseMatrix& A);
    CholeskyDecomp(const CholeskyDecomp& A) = delete;

    CholeskyDecomp& operator=(const CholeskyDecomp&) = delete;

    /** Destructor */
    virtual ~CholeskyDecomp();

    /** Return the L matrix (L*L' = M), as a dense matrix. */
    inline CMatrixDouble get_L() const
    {
      CMatrixDouble L;
      get_L(L);
      return L;
    }

    /** Return the L matrix (L*L' = M), as a dense matrix. */
    void get_L(CMatrixDouble& out_L) const;

    /** Return the vector from a back-substitution step that solves: Ux=b */
    template <class VECTOR>
    inline VECTOR backsub(const VECTOR& b) const
    {
      VECTOR res;
      backsub(b, res);
      return res;
    }

    /** Return the vector from a back-substitution step that solves: Ux=b.
     * Vectors can be Eigen::VectorXd or mrpt::math::CVectorDouble   */
    void backsub(const CVectorDouble& b, CVectorDouble& result_x) const;

    /** overload for double pointers which assume the user has reserved the
     * output memory for \a result */
    void backsub(const double* b, double* result, size_t N) const;

    /** Update the Cholesky factorization from an updated vesion of the
     * original input, square definite-positive sparse matrix.
     *  NOTE: This new matrix MUST HAVE exactly the same sparse structure
     * than the original one.
     */
    void update(const CSparseMatrix& new_SM);
  };
  static_assert(
      !std::is_copy_constructible_v<CholeskyDecomp> && !std::is_copy_assignable_v<CholeskyDecomp>,
      "Copy Check");

  /** @} */

};  // end class CSparseMatrix
}  // namespace mrpt::math
