/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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:
extern "C"
{
#if MRPT_HAS_SUITESPARSE
#define NCOMPLEX  // In MRPT we don't need complex numbers, so avoid the
// annoying warning: 'cs_ci_house' has C-linkage specified,
// but returns UDT 'std::complex<double>' which is
// incompatible with C
#include "cs.h"
#else
#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(const size_t nRows = 0, const 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(const size_t nRows = 1, const 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(const size_t row, const 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(
		const size_t row, const 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(
		const size_t row, const 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 writen 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.
	 */
	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(const size_t nRows)
	{
		ASSERT_(nRows >= (size_t)sparse_matrix.m);
		sparse_matrix.m = nRows;
	}
	inline void setColCount(const 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, const 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
