/*---------------------------------------------------------------
	FILE: CMatrixTemplate.h
	USE: See doxygen docs

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CMatrixTemplate_H
#define CMatrixTemplate_H

#include <MRPT/UTILS/utils_defs.h>

namespace UTILS
{
/**  This template class provides the basic functionality for a general 2D any-size, resizable container of numerical or non-numerical elements.
 * NOTES:
 *		- This class is not serializable since it is a template. Use or declare derived classes for using serialization.
 *		- First row or column index is "0".
 *		- This class includes range checks with ASSERT_() if compiling with "_DEBUG" or "MRPT_ALWAYS_CHECKS_DEBUG=1".
 *		- Please DO NOT use as template class type any other class. It can be safely used the following types:
 *			- Elemental types (int,char,float,doble,...)
 *			- Data struct (Not classes!)
 *			- Any kind of pointers (user is responsible for allocating and freeing the memory addressed by pointers).
 *
 * \sa CMatrixTemplateNumeric
 */
template <class T>
class CMatrixTemplate
{
protected:
	T				**m_Val;
	size_t			m_Rows, m_Cols;

	/** Internal use only: It realloc the memory for the 2D matrix, maintaining the previous contents if posible.
	  */
    void realloc(size_t row, size_t col, bool newElementsToZero = false)
	{
		if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
		{
			size_t	r;
			bool    doZeroColumns   = newElementsToZero && (col>m_Cols);
			size_t	sizeZeroColumns = sizeof(T)*(col-m_Cols);

			// If we are reducing rows, free that memory:
            for (r=row;r<m_Rows;r++)
			{
				free( m_Val[r] );
			}

			// Realloc the vector of pointers:
			m_Val = (T**)::realloc(m_Val, sizeof(T*) * row );

			// How many new rows/cols?
			size_t	row_size = col * sizeof(T);

			// Alloc new ROW pointers & resize previously existing rows, as required:
			for (r=0;r<row;r++)
			{
				if (r<m_Rows)
				{
					// This was an existing row: Resize the memory:
					m_Val[r] = (T*)::realloc( m_Val[r], row_size );

					if (doZeroColumns)
					{
						// Fill with zeros:
						::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
						// for (c=m_Cols;c<col;c++)	m_Val[r][c]= (T)(0);
					}
				}
				else
				{
					// This is a new row, alloc the memory for the first time:
					m_Val[r] = (T*)::calloc( row_size, 1 );
				}
			}

			// Done!
			m_Rows	= row;
			m_Cols	= col;
		}
	}

public:
	/** Returns a given column to a vector (without modifying the matrix)
		* \exception std::exception On index out of bounds
		*/
	void  extractCol(size_t nCol, std::vector<T> &out, int startingRow = 0)
	{
		size_t		i,n;
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (nCol>=getColCount())
			THROW_EXCEPTION("extractCol: Column index out of bounds");
#endif

		n = getRowCount() - startingRow;
		out.resize( n );

		for (i=0;i<n;i++)
			out[i] = m_Val[i+startingRow][nCol];
	}

	/** Gets a given column to a vector (without modifying the matrix)
		* \exception std::exception On index out of bounds
		*/
	void  extractCol(size_t nCol, CMatrixTemplate<T> &out, int startingRow = 0)
	{
		size_t		i,n;
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (nCol>=getColCount())
			THROW_EXCEPTION("extractCol: Column index out of bounds");
#endif

		n = getRowCount() - startingRow;
		out.setSize(n,1);

		for (i=0;i<n;i++)
			out(i,0) = m_Val[i+startingRow][nCol];
	}

	/** Gets a given row to a vector (without modifying the matrix)
		* \exception std::exception On index out of bounds
		*/
	template <class F>
	void  extractRow(size_t nRow, std::vector<F> &out, size_t startingCol = 0)
	{
		size_t		i,n;
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (nRow>=getRowCount())
			THROW_EXCEPTION("extractRow: Row index out of bounds");
#endif
		n = getColCount() - startingCol ;
		out.resize( n );

		for (i=0;i<n;i++)
			out[i] = (F) m_Val[nRow][i+startingCol];
	}

	/** Gets a given row to a vector (without modifying the matrix)
		* \exception std::exception On index out of bounds
		*/
	template <class F>
	void  extractRow(size_t nRow, CMatrixTemplate<T> &out, size_t startingCol = 0)
	{
		size_t		i,n;
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (nRow>=getRowCount())
			THROW_EXCEPTION("extractRow: Row index out of bounds");
#endif
		n = getColCount() - startingCol ;
		out.setSize(1,n);

		for (i=0;i<n;i++)
			out(0,i) = (F) m_Val[nRow][i+startingCol];
	}

	/** Constructors
	*/
	CMatrixTemplate (const CMatrixTemplate& m) : m_Val(NULL),m_Rows(0),m_Cols(0)
	{
		(*this) = m;
	}

	CMatrixTemplate (size_t row = 3, size_t col = 3) :  m_Val(NULL),m_Rows(0),m_Cols(0)
	{
		realloc(row,col);
	}

	/** Destructor
	*/
	virtual ~CMatrixTemplate()
	{
		realloc(0,0);
	}

	/** Assignment operator
	*/
	void  operator = (const CMatrixTemplate& m)
	{
		realloc( m.getRowCount(), m.getColCount() );

		for (size_t i=0; i < getRowCount(); i++)
			for (size_t j=0; j < getColCount(); j++)
				m_Val[i][j] = m.m_Val[i][j];
	}

	/** Returns size of matrix
	*/
	size_t getRowCount() const
	{
		return m_Rows;
	}

	/** Returns size of matrix
	*/
	size_t getColCount() const
	{
		return m_Cols;
	}

	/** Changes the size of matrix, maintaining the previous contents.
	*/
	virtual void setSize(size_t row, size_t col)
	{
		realloc(row,col);
	}

	/** Check for type of matrix
	*/
	bool IsSquare ()
	{
		return ( m_Rows == m_Cols );
	}

	/** Subscript operator to get/set individual elements
		*/
	T& operator () (size_t row, size_t col)
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (row >= m_Rows || col >= m_Cols)
		{
			char	str[200];
			MRPT_OS::sprintf(str,199,"Indexes (%u,%u) out of range!",row,col);
			THROW_EXCEPTION( str );
		}
#endif
		return m_Val[row][col];
	}

	/** Subscript operator to get/set individual elements
		*/
	T operator () (size_t row, size_t col) const
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (row >= m_Rows || col >= m_Cols)
			THROW_EXCEPTION( "Index out of range!");
#endif
		return m_Val[row][col];
	}

	/** Fast but unsafe method to write a value in the matrix
		*/
	inline void set_unsafe(size_t row, size_t col,const T &v)
	{
		m_Val[row][col] = v;
	}

	/** Fast but unsafe method to read a value from the matrix
		*/
	inline T get_unsafe(size_t row, size_t col) const
	{
		return m_Val[row][col];
	}

	/** Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in time critical applications)
		*/
	inline T* get_unsafe_row(size_t row) const
	{
		return m_Val[row];
	}

	/** Inserts a row from a vector, replacing the current contents of that row.
		* \exception std::exception On index out of bounds
		* \sa extractRow
		*/
	void  insertRow(size_t nRow, const std::vector<T> &in)
	{
		size_t		i,n;
		if (nRow>=getRowCount()) THROW_EXCEPTION("insertRow: Row index out of bounds");

		n = getColCount();
		ASSERT_(in.size()==getColCount());

		for (i=0;i<n;i++)
			m_Val[nRow][i] = in[i];
	}

	/** Appends a new row to the MxN matrix from a 1xN vector.
		* \exception std::exception On incorrect vector length.
		* \sa extractRow
		*/
	void  appendRow(const std::vector<T> &in)
	{
		size_t		i,n, row;

		n = getColCount();
		row = getRowCount();
		ASSERT_(in.size()==getColCount());

		realloc( row+1,n );

		for (i=0;i<n;i++)
			m_Val[row][i] = in[i];
	}

	/** Inserts a column from a vector, replacing the current contents of that column.
		* \exception std::exception On index out of bounds
		* \sa extractCol
		*/
	void  insertCol(size_t nCol, const std::vector<T> &in)
	{
		size_t		i,n;
		if (nCol>=getColCount()) THROW_EXCEPTION("insertCol: Row index out of bounds");

		n = getRowCount();
		ASSERT_(in.size()==getRowCount());

		for (i=0;i<n;i++)
			m_Val[i][nCol] = in[i];
	}

	/** Inserts a matrix into this matrix
	    *  Notice that the matrix must "fit" into the existing size of this matrix.
		* \exception std::exception On index out of bounds
		* \sa extractCol
		*/
	void  insertMatrix(size_t nRow, size_t nCol, const CMatrixTemplate<T> &in)
	{
		size_t	i,j,ncols,nrows;

		nrows = in.getRowCount();
		ncols = in.getColCount();
		if ( (nRow+nrows > getRowCount()) | (nCol+ncols >getColCount()) )
			THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");

		for (i=nRow;i<nRow+nrows;i++)
			for(j=nCol;j<nCol+ncols;j++)
				m_Val[i][j] = in.get_unsafe(i-nRow,j-nCol);
	}

	/** Inserts a matrix line (vector) into this matrix
	    *  Notice that the matrix must "fit" into the existing size of this matrix.
		* \exception std::exception On index out of bounds
		* \sa extractCol
		* By AJOGD @ MAR-2007
		*/
	void  insertMatrix(size_t nRow, size_t nCol, const std::vector<T> &in)
	{
		size_t	j,ncols;

		ncols = in.size();
		if ( (nRow+1 > getRowCount()) | (nCol+ncols >getColCount()) )
			THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");

		for(j = nCol ; j < nCol + ncols ; j++)
			m_Val[nRow][j] = in[j-nCol];
	}

	/** Inserts 4 matrixes corresponding to the "four corners" into this matrix.
		* \exception std::exception On index out of bounds
		* \sa insertMatrix
		*/
	void  joinMatrix(const CMatrixTemplate<T> &left_up,		const CMatrixTemplate<T> &right_up,
								const CMatrixTemplate<T> &left_down,	const CMatrixTemplate<T> &right_down)
	{
		if ((left_up.getRowCount()!= right_up.getRowCount())|(left_up.getColCount()!=left_down.getColCount())|
			(left_down.getRowCount()!=right_down.getRowCount())|(right_up.getColCount()!=right_down.getColCount()))
			THROW_EXCEPTION("join_Matrix: Row or Col index out of bounds");
		setSize(left_up.getRowCount()+left_down.getRowCount(),left_up.getColCount()+right_up.getColCount());
		insertMatrix(0,0,left_up);
        insertMatrix(0,left_up.getColCount(),right_up);
        insertMatrix(left_up.getRowCount(),0,left_down);
        insertMatrix(left_up.getRowCount(),left_up.getColCount(),right_down);
	}

	/** Fill the matrix with a given value:
	  */
	void  fill( const T &val)
	{
		for (size_t r=0;r<m_Rows;r++)
			for (size_t c=0;c<m_Cols;c++)
				m_Val[r][c]= val;
	}



}; // end of class definition

/** Textual output stream function.
  *    Use only for text output, for example:  "std::cout << mat;"
  */
template <class T>
std::ostream& operator << (std::ostream& ostrm, const CMatrixTemplate<T>& m)
{
	ostrm << std::setprecision(16);

	for (size_t i=0; i < m.getRowCount(); i++)
	{
		for (size_t j=0; j < m.getColCount(); j++)
		{
			ostrm << m(i,j) << "\t";
		}
		ostrm << std::endl;
	}
	return ostrm;
}

} // End of namespace



#endif //__STD_MATRIX_H
