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

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

#include <MRPT/UTILS/CMatrixTemplate.h>

namespace UTILS
{
/**  This template class extends the class "CMatrixTemplate" with many common operations with numerical matrixes.
 *
 *   The following operators have been implemented:
<P>
<TABLE cellSpacing=1 cellPadding=1 width="75%" align=center border=1>
  <TR>
    <TD align=center width="30%"><b>Implemented Operators</b></TD>
    <TD width="70%"><center><b>Meaning</b></center></TD></TR>
  <TR>
    <TD align=center>x=M(i,j)<br>M(i,j)=x</TD>
    <TD>This () operator is used to access/change the element at i'th row, j'th column. First index is 0.</TD></TR>
  <TR>
    <TD align=center>!M</TD>
    <TD>The matrix inverse M<sup>-1</sup></TD></TR>
  <TR>
    <TD align=center>~M</TD>
    <TD>The matrix transpose M<sup>T</sup></TD></TR>
  <TR>
    <TD align=center>(M^n)</TD>
    <TD>Power of a matrix: (M*M*M*...M) n times. Use parenthesis with this operator. Use "scalarPow" for the power of individual elements in the matrix. </TD></TR>
  <TR>
    <TD align=center> M1 = M2</TD>
    <TD>Assignment operator: Copy matrix M2 to M1</TD></TR>
  <TR>
    <TD align=center>M1 == M2<br>M1 != M2</TD>
    <TD>Logical comparison: Returns true or false if all elements are identical.</TD></TR>
  <TR>
    <TD align=center>x * M</TD>
    <TD>Scalar multiplication</TD></TR>
  <TR>
    <TD align=center>M1 * M2</TD>
    <TD>Matrix multiplication, with the usual mathematical meaning</TD></TR>
  <TR>
    <TD align=center>M1 + M2<br>M1  M2</TD>
    <TD>Matrixes addition and substraction.</TD></TR>
  <TR>
    <TD align=center>M / x</TD>
    <TD>Scalar division</TD></TR>
 <TR>
    <TD align=center>M1 / M2</TD>
    <TD>Equivalent to (M1 * M2<sup>-1</sup>)</TD></TR>
 <TR>
    <TD align=center>stream &lt;&lt; M;</TD>
    <TD>Write to a binary CStream, since this class is CSerializable</TD></TR>
 <TR>
    <TD align=center>stream &gt;&gt; M;</TD>
    <TD>Read from a binary CStream, since this class is CSerializable</TD></TR>
</TABLE>
</P>
 *  See also this useful methods:
 *		- DEBUG_SAVE_MATRIX
 * \sa CMatrixTemplate
 */
template <class T>
class CMatrixTemplateNumeric : public CMatrixTemplate<T>
{
private:

public:
	/** Copy constructor
	*/
	CMatrixTemplateNumeric(const CMatrixTemplate<T>& m) : CMatrixTemplate<T>( m )
	{
	}

	/** Constructor
	*/
	CMatrixTemplateNumeric(size_t row = 3, size_t col = 3) :  CMatrixTemplate<T>( row, col )
	{
	}

	/** Assignment operator
	*/
	void  operator = (const CMatrixTemplateNumeric& m)
	{
		CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() );

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

	/** Changes the size of matrix
	*/
	virtual void setSize(size_t row, size_t col)
	{
        CMatrixTemplate<T>::realloc(row,col,true);
	}

	/** Save matrix to a text file, compatible with MATLAB text format.
	    * \param fileFormat 0(default)=engineering format '%e', 1=Floating number format '%f', 2=Integers '%i'
		* \sa loadFromTextFile, DEBUG_SAVE_MATRIX
		*/
	void  saveToTextFile(
		const std::string &file,
		int   fileFormat = 0) const
	{
		MRPT_TRY_START;

		FILE	*f=MRPT_OS::fopen(file.c_str(),"wt");
		if (!f)
			THROW_EXCEPTION("saveToTextFile: Error opening file for writing matrix as text!");

		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
		{
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
			{
				switch(fileFormat)
				{
				case 0: MRPT_OS::fprintf(f,"%.16le",(double) (this->operator() (i,j)) ); break;
				case 1: MRPT_OS::fprintf(f,"%f",(float) (this->operator() (i,j)) ); break;
				case 2: MRPT_OS::fprintf(f,"%i",(int) (this->operator() (i,j)) ); break;
				default:
					THROW_EXCEPTION("Unsupported value for the parameter 'fileFormat'!");
				};
				// Separating blank space
				if (j<(CMatrixTemplate<T>::m_Cols-1)) MRPT_OS::fprintf(f," ");
			}
			MRPT_OS::fprintf(f,"\n");
		}
		MRPT_OS::fclose(f);

		MRPT_TRY_END;
	}


	/** Load matrix from a text file, compatible with MATLAB text format.
		* \sa saveToTextFile
		*/
	void  loadFromTextFile(const std::string &file)
	{
		FILE	*f=MRPT_OS::fopen(file.c_str(),"rt");
		if (!f)
			THROW_EXCEPTION("loadFromTextFile: can't open file!");

		char	str[50000];
		double	fil[10000];

		char				*ptr,*ptrEnd;
		size_t				i,j;
		int					nCols = -1;
		size_t				nRows = 0 ;


		CMatrixTemplate<T>::realloc(0,0);

		while (!feof(f))
		{
			if ( fgets(str,49999,f) )
			{
				// Parse row to floats:
				ptr = str;
				ptrEnd = NULL;
				i=0;

				// Process each number in this row:
				while ( ptr[0] && ptr!=ptrEnd )
				{
					// Find next number: (non white-space character):
					while (ptr[0] && (ptr[0]==' ' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
						ptr++;

					// Convert to "double":
					fil[i] = strtod(ptr,&ptrEnd);

					// A valid conversion has been done?
					if (ptr!=ptrEnd)
					{
						i++;			// Yes!
						ptr = ptrEnd;
						ptrEnd = NULL;
					}

				}; // end while procesing this row

				if (nCols<0)
				{
					// First row:
					nCols = (int)i;
					CMatrixTemplate<T>::realloc(nCols,nCols);
				}
				else
				{
					// Same elements count in each row?
					if (CMatrixTemplate<T>::getColCount()!=i )
						THROW_EXCEPTION("The matrix in the text file must have the same number of elements in each row!");
				}

				// Copy row to matrix:
				for (j=0;j<(size_t)nCols;j++)
					CMatrixTemplate<T>::m_Val[nRows][j] = (T)fil[j];

				nRows++;
				if (nRows >= CMatrixTemplate<T>::getRowCount() )
					CMatrixTemplate<T>::realloc( nRows+10, nCols );

			} // end if fgets

		} // end while not feof

		if (nRows && nCols)
			CMatrixTemplate<T>::realloc( nRows, nCols );

		MRPT_OS::fclose(f);

		// Report error as exception
		if ( !CMatrixTemplate<T>::getRowCount() || !CMatrixTemplate<T>::getColCount() )
			THROW_EXCEPTION("loadFromTextFile: Error loading from text file");
	}



	/** Computes the laplacian of the matrix, useful for graph matrixes.
	 */
	CMatrixTemplateNumeric<T>  laplacian() const
	{
		if ( CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols ) THROW_EXCEPTION( "laplacian: Defined for square matrixes only!");

		CMatrixTemplateNumeric<T>		ret;
		size_t							i,j,size;
		T								v;

		size = CMatrixTemplate<T>::m_Rows;

		// Compute the "degree" of each node:
		// -------------------------------------
		T  *deg = new T[size];

		for (i=0;i<size;i++)
		{
				deg[i] = 0;
				for (j=0;j<size;j++)
						deg[i] += CMatrixTemplate<T>::m_Val[j][i];
		}

		// Compute lapplacian (normalized)
		//   LAPLACIAN = D - W
		// -----------------------------------
		ret.realloc(size,size);

		for(i=0;i<size;i++)
		{
			ret(i,i) = deg[i] - CMatrixTemplate<T>::m_Val[i][i];

			for(j=i+1;j<size;j++)
			{
				v = - CMatrixTemplate<T>::m_Val[i][j];
				ret(i,j) = v;
				ret(j,i) = v;
			}
		}

		// Free memory:
		delete[] deg;

		return ret;
	}

	/** Computes the SVD (Singular Value Decomposition) of the matrix.
	  *  If "this" matrix is named A with dimensions M x N, this method computes: <br>
	  *			A = U * W * V' <br>
	  * <br>
	  *  , where U is a M x N column orthogonal matrix, W is a diagonal matrix
	  *  containing the singular values, and V is a NxN matrix. <br>
	  * This method returns the U matrix, the N elements in the diagonal of W as a vector,
	  *  and the matrix V, NOT TRANSPOSED.
	  */
	void  svd(CMatrixTemplateNumeric<T> &U, std::vector<T> &W,CMatrixTemplateNumeric<T> &V) const
	{
		// Algorithm from "Numerical recipes in C"
		// void  CMatrixD::svdcmp(double* a[], int m, int n, double w[], double* v[])
		//
		//  a <-- this,
		//    execute svdcmp
		//  U <-- a
		//  W <-- w
		//  V <-- v
		// -----------------------------------------------

		int			i,j;
		T			**a, **v;
		T			*w;
		int			m = CMatrixTemplate<T>::getRowCount(),n=CMatrixTemplate<T>::getColCount();

		// Copy the matrix content to "a":
		// --------------------------------------
		typedef T* matrix_type_ptr;

		w = new T[n];
		a = new matrix_type_ptr[m];
		v = new matrix_type_ptr[n];
		for (i=0;i<m;i++)	a[i] = new T[n];
		for (i=0;i<n;i++)	v[i] = new T[n];

		for (i=0;i<m;i++)
			for (j=0;j<n;j++)
				a[i][j] = (*this)(i,j);

		// Algorithm
		// --------------------------------------
		svdcmp(a,m,n,w,v);

		// Copy results to matrices classes
		// --------------------------------------
		W.resize( n );
		V.setSize(n,n);

		for (i=0;i<m;i++)
			for (j=0;j<n;j++)
				U(i,j)= a[i][j];

		for (i=0;i<n;i++)
			W[i] = w[i];

		for (i=0;i<n;i++)
			for (j=0;j<n;j++)
				V(i,j)=v[i][j];

		// Free
		// --------------------------------------
		for (i=0;i<m;i++)	delete[] a[i];
		for (i=0;i<n;i++)	delete[] v[i];
		delete[]	a;
		delete[]	v;
		delete[]	w;
	}

	/** Computes the eigenvalues/eigenvector decomposition of a symmetric matrix.
	 *    The decomposition is: M = Z · D · Z<sup>T</sup>, where columns in Z are the
	 *	  eigenvectors and the diagonal matrix D contains the eigenvalues
	 *    as diagonal elements, sorted in <i>ascending</i> order.
	 *    The algorithm is taken from "Numerical recipes in C", freely available online.
	 */
    void  eigenVectors(CMatrixTemplateNumeric<T> &Z, CMatrixTemplateNumeric<T> &D) const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION( "eigenVectors: Only for square matrixes!");

		std::vector<int>	indxs;
		std::vector<bool>	already;

		CMatrixTemplateNumeric<T>		save( *this );
		CMatrixTemplateNumeric<T>		E;			// Diagonal.
		size_t								i,j;
		size_t								n;
		T								**a;
		T								*d,*e;

		MRPT_TRY_START;

		// Algorithm from "Numerical recipes in C"
		// -----------------------------------------------
		n = CMatrixTemplate<T>::m_Cols;

		// Check for symmetry
		// --------------------------------------
		for (i=0;i<n;i++)
			for (j=0;j<n;j++)
				if (CMatrixTemplate<T>::m_Val[i][j]!=CMatrixTemplate<T>::m_Val[j][i])
				{
					char	str[1000];
					MRPT_OS::sprintf(str,999,"eigenVectors: The matrix is not symmetric! m(%u,%u)=%.16e != m(%u,%u)=%.16e\n",
						i,
						j,
						(double)CMatrixTemplate<T>::m_Val[i][j],
						j,
						i,
						(double)CMatrixTemplate<T>::m_Val[j][i]);
					THROW_EXCEPTION(str);
				}

		// Copy the matrix content to "a":
		// --------------------------------------
		typedef T* matrix_type_ptr;

		a = new matrix_type_ptr[n+1];
		for (i=1;i<=n;i++)	a[i] = new T[n+1];
		d = new T[n+1];
		e = new T[n+1];

		for (i=1;i<=n;i++)
			for (j=1;j<=n;j++)
				a[i][j] = CMatrixTemplate<T>::m_Val[i-1][j-1];

		// Algorithm
		// --------------------------------------
		tred2( a, n, d, e);

		tqli(d,e,n,a);

		// In "d" are the eigenvalues
		// In "a" are the eigenvectors as columns:

		// SORT: Build a list of the n index in
		//   ascending order of eigenvalue:
		// --------------------------------------
		indxs.resize(n+1);
		already.resize(n+1, false);

		for (i=1;i<=n;i++)
		{
			int		minIndx = -1;
			for (j=1;j<=n;j++)
				if (!already[j])
				{
					if (minIndx==-1)			minIndx = (int)j;
					else
						if (d[j]<d[minIndx])	minIndx = (int)j;
				}

			// The i'th sorted element:
			indxs[i] = minIndx;
			already[minIndx] = true;
		}

		for (i=1;i<=n;i++)
			ASSERT_(already[i]);

		// Copy results to matrices classes
		// --------------------------------------
		Z.setSize(n,n);
		D.setSize(n,n);
		for (i=1;i<=n;i++)
			for (j=1;j<=n;j++)
			{
				Z(i-1,j-1) = a[i][indxs[j]];
				if (i==j)
				{
					if (d[indxs[j]]<0)
							D(i-1,i-1) = -d[indxs[j]];
					else	D(i-1,i-1) = d[indxs[j]];
				}
				else		D(i-1,j-1) = 0;

				// Debug:
				ASSERT_( !(UTILS::MRPT_OS::isNaN( Z(i-1,j-1) )));
				ASSERT_( !(UTILS::MRPT_OS::isNaN( D(i-1,i-1) )));
			}

		// Free
		// --------------------------------------
		for (i=1;i<=n;i++)	delete[] a[i];
		delete[]	a;
		delete[]	d;
		delete[]	e;

		*((CMatrixTemplateNumeric<T>*)this) = save;

		MRPT_TRY_END_WITH_CLEAN_UP( \
			std::cout << "[eigenVectors] The matrix leading to exception is:" << std::endl << save << std::endl; \
			*((CMatrixTemplateNumeric<T>*)this) = save; \
			);

	}

	/** Efficiently computes only the biggest eigenvector of the matrix using the Power Method, and returns it as a column vector.
	  *  The computation time for this method, in a Pentium 4 1.4Ghz is:<br>
	  *         T = 7.0867e-008*n<sup>2</sup> + 1.9191e-005*n + 0.0017494 seconds  <br>
	  *  where N is the matrix size.
	  */
	CMatrixTemplateNumeric<T>  largestEigenvector(
		T			resolution = 0.01f,
		int			maxIterations = 6,
		int			*out_Iterations = NULL,
		float		*out_estimatedResolution = NULL ) const
	{
		// Apply the iterative Power Method:
		// -------------------------------------
		int							i, iter=0, n = CMatrixTemplate<T>::m_Rows;
		CMatrixTemplateNumeric<T>	x,xx;		// The iterative vector
		T							norm,dif;

		// Initially, set to ones for example.
		x.ones(n,1);

		// Iterative loop:
		do
		{
			xx = (*this) * x;

			// Normalize:
			norm = 0;
			for (i=0;i<n;i++)
				norm+=square(xx(i,0));
			xx *= (T)(1.0/::sqrt(norm));

			// Compute diference between iterations:
			dif = 0;
			for (i=0;i<n;i++)
				dif+=(T)( square(fabs(xx(i,0)-x(i,0))));
			dif=(T)::sqrt(dif);

			// Set as current estimation:
			x = xx;

			// Iteration counter:
			iter++;

		} while (iter<maxIterations && dif>resolution);

		if (out_Iterations) *out_Iterations=iter;
		if (out_estimatedResolution) *out_estimatedResolution=dif;

		// OK:
		return x;
	}

	/** Computes the sqrt of each element in the matrix.
		*/
	void Sqrt()
	{
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
		{
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
			{
			if (CMatrixTemplate<T>::m_Val[i][j]>0)
				CMatrixTemplate<T>::m_Val[i][j] = (T) ::sqrt(CMatrixTemplate<T>::m_Val[i][j]);
			else	CMatrixTemplate<T>::m_Val[i][j] = 0;
			}
		}
		return;
	}

   /** Unary operator
    */
   CMatrixTemplateNumeric<T>  operator + ()
   {
	   return (*this);
   }

   /** Unary operator
    */
	CMatrixTemplateNumeric<T>  operator - ()
	{
		CMatrixTemplateNumeric<T>	temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);

		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				temp.m_Val[i][j] = - CMatrixTemplate<T>::m_Val[i][j];

		return temp;
	}

	/** combined addition and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator += (const CMatrixTemplateNumeric<T>& m)
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (CMatrixTemplate<T>::m_Rows != m.m_Rows || CMatrixTemplate<T>::m_Cols != m.m_Cols)
			THROW_EXCEPTION( "operator+= : Inconsistent matrix sizes in addition!");
#endif

		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] += m.m_Val[i][j];
		return *this;
	}

	/** combined subtraction and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator -= (const CMatrixTemplateNumeric<T>& m)
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (CMatrixTemplate<T>::m_Rows != m.m_Rows || CMatrixTemplate<T>::m_Cols != m.m_Cols)
			THROW_EXCEPTION( "operator+= : Inconsistent matrix sizes in addition!");
#endif
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] -= m.m_Val[i][j];
		return *this;
	}


	/** combined scalar multiplication and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator *= (const T& c)
	{
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] *= c;
		return *this;
	}

	/** combined matrix multiplication and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator *= (const CMatrixTemplateNumeric<T>& m)
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (CMatrixTemplate<T>::m_Cols != m.m_Rows)
			THROW_EXCEPTION( "operator*= : Inconsistent matrix sizes in multiplication!");
#endif
		CMatrixTemplateNumeric<T>	temp(CMatrixTemplate<T>::m_Rows,m.m_Cols);

		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < m.m_Cols; j++)
			{
				temp.m_Val[i][j] = (T)0;
				for (size_t k=0; k < CMatrixTemplate<T>::m_Cols; k++)
				temp.m_Val[i][j] += CMatrixTemplate<T>::m_Val[i][k] * m.m_Val[k][j];
			}
		*this = temp;
		return *this;
	}

	/** Dot division by another matrix (element by element division)
	  */
	CMatrixTemplateNumeric<T>&  operator /= (const CMatrixTemplateNumeric<T>& m)
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		if (CMatrixTemplate<T>::m_Cols != m.m_Cols || CMatrixTemplate<T>::m_Rows != m.m_Rows )
			THROW_EXCEPTION( "operator/= : Matrixes are not of the same dimensions!");
#endif
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < m.m_Cols; j++)
				if (m.m_Val[i][j]!=0)
				{
					CMatrixTemplate<T>::m_Val[i][j] /= m.m_Val[i][j];
				}
		return *this;
	}


	/** combined scalar division and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator /= (const T& c)
	{
		MRPT_TRY_START;
		if (c==0) THROW_EXCEPTION("Aborted! Trying to divide by zero!");

		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] /= c;
		return *this;

		MRPT_TRY_END;
	}

	/** Combined scalar addition and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator += (const T& c)
	{
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] += c;
		return *this;
	}

	/** Combined scalar substraction and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator -= (const T& c)
	{
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] -= c;
		return *this;
	}

	/** combined power and assignment operator
	*/
	CMatrixTemplateNumeric<T>&  operator ^= (const unsigned int& pow)
	{
		CMatrixTemplateNumeric<T> temp(*this);

		for (size_t i=2; i <= pow; i++)
			*this = *this * temp;

		return *this;
	}

	/** Scalar power of all elements to a given power, this is diferent of ^ operator.
		*/
	void  scalarPow(T s)
	{
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] = pow(CMatrixTemplate<T>::m_Val[i][j],s);
	}

   /** Set all elements to zero
    */
   void  zeros(const size_t& row, const size_t& col)
   {
	   setSize(row,col);
	   zeros();
   }

   /** Set all elements to zero
    */
   void  zeros()
   {
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] = (T)0;
   }

   /** Set all elements to one
    */
   void  ones(const size_t& row, const size_t& col)
   {
	   setSize(row,col);
	   ones();
   }

   /** Set all elements to one
    */
   void  ones()
   {
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] = (T)1;
   }

   /** Build an unit matrix.
    */
   void  unit (const size_t& row)
   {
	   setSize(row,row);
	   unit();
   }

   /** Build an unit matrix.
    */
   void  unit()
   {
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				CMatrixTemplate<T>::m_Val[i][j] = (i==j) ? ((T)1) : ((T)0);
   }

	/** Solve the matrix as linear equations system.
	*/
	CMatrixTemplateNumeric<T>  solve (const CMatrixTemplateNumeric<T>& v)
	{
		size_t	i,j,k;
		T				a1;

		if (!(CMatrixTemplate<T>::m_Rows == CMatrixTemplate<T>::m_Cols && CMatrixTemplate<T>::m_Cols == v.m_Rows))
			THROW_EXCEPTION( "solve:Inconsistent matrices!");

		CMatrixTemplateNumeric<T>	temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols+v.m_Cols);
		for (i=0; i < CMatrixTemplate<T>::m_Rows; i++)
		{
			for (j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				temp.m_Val[i][j] = CMatrixTemplate<T>::m_Val[i][j];
			for (k=0; k < v.m_Cols; k++)
				temp.m_Val[i][CMatrixTemplate<T>::m_Cols+k] = v.m_Val[i][k];
		}
		for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
		{
			int indx = temp.pivot(k);
			if (indx == -1)
			{
				std::cout << "[solve] Matrix that leaded to error is:" << std::endl << (*this) << std::endl;
				THROW_EXCEPTION( "solve: Singular matrix!");
			}

			a1 = temp.m_Val[k][k];
			for (j=k; j < temp.m_Cols; j++)
				temp.m_Val[k][j] /= a1;

			for (i=k+1; i < CMatrixTemplate<T>::m_Rows; i++)
			{
				a1 = temp.m_Val[i][k];
				for (j=k; j < temp.m_Cols; j++)
					temp.m_Val[i][j] -= a1 * temp.m_Val[k][j];
			}
		}

		CMatrixTemplateNumeric<T>	s(v.m_Rows,v.m_Cols);
		for (k=0; k < v.m_Cols; k++)
			for (int m=int(CMatrixTemplate<T>::m_Rows)-1; m >= 0; m--)
			{
				s.m_Val[m][k] = temp.m_Val[m][CMatrixTemplate<T>::m_Cols+k];
				for (j=m+1; j < CMatrixTemplate<T>::m_Cols; j++)
					s.m_Val[m][k] -= temp.m_Val[m][j] * s.m_Val[j][k];
			}
		return s;
	}

	/** Computes the adjunt of matrix.
	*/
	CMatrixTemplateNumeric<T>  adj() const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION( "adj: Adjoin of a non-square matrix.");

		CMatrixTemplateNumeric<T>	temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);

		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				temp.m_Val[j][i] = CMatrixTemplate<T>::Cofact(i,j);
		return temp;
	}

	/** Computes the Inverse of matrix.
	*	Return the inverse of a matrix without modifying the original matrix
	*/
	CMatrixTemplateNumeric<T>  inv() const
	{
		size_t	i,j,k;
		T				a1,a2,*rowptr;

		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION( "operator!: Inversion of a non-square matrix");

		CMatrixTemplateNumeric<T>	temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);
		CMatrixTemplateNumeric<T>	temp2( *this );

		temp.unit();
		for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
		{
			int indx = temp2.pivot(k);
			if (indx == -1)
			{
				std::cout << "[inv] Matrix that leaded to error is:" << std::endl << (*this) << std::endl;
				THROW_EXCEPTION( "operator!: Inversion of a singular matrix");
			}

			if (indx != 0)
			{
				rowptr = temp.m_Val[k];
				temp.m_Val[k] = temp.m_Val[indx];
				temp.m_Val[indx] = rowptr;
			}
			a1 = temp2.m_Val[k][k];
			for (j=0; j < temp2.m_Rows; j++)
			{
				temp2.m_Val[k][j] /= a1;
				temp.m_Val[k][j] /= a1;
			}
			for (i=0; i < temp2.m_Rows; i++)
			{
				if (i != k)
				{
					a2 = temp2.m_Val[i][k];
					for (j=0; j < temp2.m_Rows; j++)
					{
						temp2.m_Val[i][j] -= a2 * temp2.m_Val[k][j];
						temp.m_Val[i][j] -= a2 * temp.m_Val[k][j];
					}
				}
			}
		}
		return temp;
	}
/*********************************************************AJOGD********************************************************/
/** Computes the Inverse of matrix.
	*	Calculate the inverse of a matrix overwriting the original matrix
	*/
	void  inv_unsafe()
	{
		size_t	i,j,k;
		T				a1,a2,*rowptr;

		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION( "operator!: Inversion of a non-square matrix");

		CMatrixTemplateNumeric<T>	temp(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);

		temp.unit();
		for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
		{
			int indx = (*this).pivot(k);
			if (indx == -1)
			{
				std::cout << "[inv] Matrix that leaded to error is:" << std::endl << (*this) << std::endl;
				THROW_EXCEPTION( "operator!: Inversion of a singular matrix");
			}

			if (indx != 0)
			{
				rowptr = temp.m_Val[k];
				temp.m_Val[k] = temp.m_Val[indx];
				temp.m_Val[indx] = rowptr;
			}
			a1 = CMatrixTemplate<T>::m_Val[k][k];
			for (j=0; j < CMatrixTemplate<T>::m_Rows; j++)
			{
				CMatrixTemplate<T>::m_Val[k][j] /= a1;
				temp.m_Val[k][j] /= a1;
			}
			for (i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			{
				if (i != k)
				{
					a2 = CMatrixTemplate<T>::m_Val[i][k];
					for (j=0; j < CMatrixTemplate<T>::m_Rows; j++)
					{
						CMatrixTemplate<T>::m_Val[i][j] -= a2 * CMatrixTemplate<T>::m_Val[k][j];
						temp.m_Val[i][j] -= a2 * temp.m_Val[k][j];
					}
				}
			}
		}
		(*this)=temp;
	}
/**********************************************************************************************************************/

	/** Computes the determinant of matrix.
	*/
	T  det() const
	{
		size_t	i,j,k;
		T				piv,detVal = T(1);

		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION( "det: Determinant a non-square matrix!");

		CMatrixTemplateNumeric<T>	temp(*this);

		for (k=0; k < CMatrixTemplate<T>::m_Rows; k++)
		{
			int		indx = temp.pivot(k);
			if (indx == -1)
				return 0;
			if (indx != 0)
				detVal = - detVal;
			detVal = detVal * temp.m_Val[k][k];

			for (i=k+1; i < CMatrixTemplate<T>::m_Rows; i++)
			{
				piv = temp.m_Val[i][k] / temp.m_Val[k][k];
				for (j=k+1; j < CMatrixTemplate<T>::m_Rows; j++)
					temp.m_Val[i][j] -= piv * temp.m_Val[k][j];
			}
		}
		return detVal;
	}

	/** Computes the norm of matrix.
	*/
	T  norm() const
	{
		T	retVal = 0;

		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				retVal += CMatrixTemplate<T>::m_Val[i][j] * CMatrixTemplate<T>::m_Val[i][j];
		retVal = ::sqrt( retVal );
		return retVal;
	}

	/** Computes the cofact.
	*/
	T  cofact (size_t row, size_t col)
	{
		size_t i,i1,j,j1;

		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION( "cofact: Cofactor of a non-square matrix!");

		if (row > CMatrixTemplate<T>::m_Rows || col > CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION( "cofact: Index out of range!");

		CMatrixTemplateNumeric<T> temp (CMatrixTemplate<T>::m_Rows-1,CMatrixTemplate<T>::m_Cols-1);

		for (i=i1=0; i < CMatrixTemplate<T>::m_Rows; i++)
		{
			if (i == row)
				continue;
			for (j=j1=0; j < CMatrixTemplate<T>::m_Cols; j++)
			{
				if (j == col)
					continue;
				temp.m_Val[i1][j1] = CMatrixTemplate<T>::m_Val[i][j];
				j1++;
			}
			i1++;
		}
		T	cof = temp.det();
		if ((row+col)%2 == 1)
			cof = -cof;

		return cof;
	}

	/** Computes the cond.
	*/
	T	 cond()
	{
		CMatrixTemplateNumeric<T> inv = ! (*this);
		return (CMatrixTemplate<T>::Norm() * inv.norm());
	}

	/** Checks for matrix type
	  */
	bool  isSingular() const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			return false;
		return (det() == T(0));
	}

	/** Checks for matrix type
	  */
	bool  isDiagonal() const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			return false;
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				if (i != j && CMatrixTemplate<T>::m_Val[i][j] != T(0))
					return false;
		return true;
	}

	/** Checks for matrix type
	  */
	bool  isScalar() const
	{
		if (!isDiagonal())
			return false;
		T	v = CMatrixTemplate<T>::m_Val[0][0];
		for (size_t i=1; i < CMatrixTemplate<T>::m_Rows; i++)
			if (CMatrixTemplate<T>::m_Val[i][i] != v)
				return false;
		return true;
	}

	/** Checks for matrix type
	  */
	bool  isUnit() const
	{
		return (isScalar() && CMatrixTemplate<T>::m_Val[0][0] == T(1));
	}

	/** Checks for matrix type
	  */
	bool  isNull() const
	{
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
				if (CMatrixTemplate<T>::m_Val[i][j] != T(0))
					return false;
		return true;
	}

	/** Checks for matrix type
	  */
	bool  isSymmetric() const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			return false;
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
			if (CMatrixTemplate<T>::m_Val[i][j] != CMatrixTemplate<T>::m_Val[j][i])
				return false;
		return true;
	}

	/** Checks for matrix type
	  */
	bool  isSkewSymmetric() const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			return false;
		for (size_t i=0; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < CMatrixTemplate<T>::m_Cols; j++)
			if (CMatrixTemplate<T>::m_Val[i][j] != -CMatrixTemplate<T>::m_Val[j][i])
				return false;
		return true;
	}

	/** Checks for matrix type
	  */
	bool  isUpperTriangular() const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			return false;
		for (size_t i=1; i < CMatrixTemplate<T>::m_Rows; i++)
			for (size_t j=0; j < i-1; j++)
			if (CMatrixTemplate<T>::m_Val[i][j] != T(0))
				return false;
		return true;
	}

	/** Checks for matrix type
	  */
	bool  isLowerTriangular() const
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			return false;

		for (size_t j=1; j < CMatrixTemplate<T>::m_Cols; j++)
			for (size_t i=0; i < j-1; i++)
			if (CMatrixTemplate<T>::m_Val[i][j] != T(0))
				return false;

		return true;
	}

	/** Dump matrix in matlab format:
	 */
	std::string  inMatlabFormat() const
	{
		std::string	s;
		char			str[100];
		s = "[";
		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
		{
			for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
			{
				MRPT_OS::sprintf(str,100,"%le ",(double)CMatrixTemplate<T>::m_Val[i][j] );
				s=s+ std::string(str);
			}
			if (i<CMatrixTemplate<T>::m_Rows-1)	s=s+";";
		}
		s=s+"];";
		return s;
	}
	/** Round towards minus infinity modifying the matrix
	  * (by AJOGD @ JAN-2007)
	  */
	void  matrix_floor()
	{
		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
			for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
				CMatrixTemplate<T>::m_Val[i][j] = floor(CMatrixTemplate<T>::m_Val[i][j]);
	}

	/** Round towards minus infinity
	  * (by AJOGD @ JAN-2007)
	  */
	void  matrix_floor(CMatrixTemplateNumeric<T> &out)
	{
		out.setSize(CMatrixTemplate<T>::m_Rows,CMatrixTemplate<T>::m_Cols);
		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
			for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
				out(i,j) = floor(CMatrixTemplate<T>::m_Val[i][j]);
	}

	/** Round towards plus infinity
	  * (by AJOGD @ JAN-2007)
	  */
	void  matrix_ceil()
	{
		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
			for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
				CMatrixTemplate<T>::m_Val[i][j] = ceil(CMatrixTemplate<T>::m_Val[i][j]);
	}

	/** Finds the maximum value in the matrix, and returns its position.
	  * (by AJOGD @ JAN-2007)
	  */
	void  find_index_max_value(int &umax, int &vmax, T &max_val)
	{
		max_val = (*this)(0,0);
		umax = vmax = 0;
		for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
		{
			for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
			{
				if (max_val<(*this)(i,j))
				{
					max_val=(*this)(i,j);
					umax=(int)j;
					vmax=(int)i;
				}
			}
		}
	}

	/** Finds the maximum value in the matrix.
	  */
	T  maximum()
	{
		T max_val = (*this)(0,0);
		for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
			for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
				if (max_val<(*this)(i,j))
					max_val=(*this)(i,j);

		return max_val;
	}

	/** Finds the minimum value in the matrix.
	  */
	T  minimum()
	{
		T min_val = (*this)(0,0);
		for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
			for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
				if (min_val>(*this)(i,j))
					min_val =(*this)(i,j);
		return min_val;
	}

	/** Finds the minimum value in the matrix, and returns its position.
	  * (by AJOGD @ JAN-2007)
	  */
	void  find_index_min_value(int &umin, int &vmin, T &min_val)
	{
		min_val = (*this)(0,0);
		for (size_t i=0;i<CMatrixTemplate<T>::getRowCount();i++)
		{
			for(size_t j=0;j<CMatrixTemplate<T>::getColCount();j++)
			{
				if (min_val>(*this)(i,j))
				{
					min_val=(*this)(i,j);
					umin=(int)j;
					vmin=(int)i;
				}
			}
		}
	}

	/** Force symmetry in the matrix
	  * (by AJOGD @ JAN-2007)
	  */
	void  force_symmetry()
	{
		if (CMatrixTemplate<T>::m_Rows!=CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION("Error in force_symmetry. The matrix is not square");

		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows-1;i++)
		{
			for(size_t j=i+1;j<CMatrixTemplate<T>::m_Cols;j++)
			{
				(*this)(j,i) = (*this)(i,j);
			}
		}
	}


	/** Force matrix to be positive-definite
	  * (by AJOGD @ MAR-2007)
 	  */
	void  force_pd()
	{
		if (CMatrixTemplate<T>::m_Rows != CMatrixTemplate<T>::m_Cols)
			THROW_EXCEPTION("Error in CMatrixTemplateNumeric::force_pd. Matrix not square");

		CMatrixTemplateNumeric<T>	eigval,eigvec;
		T							min_val;
		int							u,v;

		(*this).eigenVectors(eigvec,eigval);
		eigval.find_index_min_value(u,v,min_val);
		if (min_val <= T(0))
		{
			min_val *= -1;
			min_val += (float)1e-9;
			for (size_t i = 0 ; i < CMatrixTemplate<T>::m_Rows ; i++)
				eigval(i,i) += min_val;
		}
		(*this) = eigvec * eigval * ~eigvec;
	}


	/** Computes a row with the mean values of each column in the matrix.
	  * \sa meanAndStdAll
	  */
	void  mean( std::vector<T> &outMeanVector )
	{
		MRPT_TRY_START;

		size_t	nCols = CMatrixTemplate<T>::getColCount();
		size_t	nRows = CMatrixTemplate<T>::getRowCount();
		ASSERT_(nCols!=0);
		ASSERT_(nRows!=0);

		// Compute the mean row:
		outMeanVector.resize(nCols);

		for (size_t c=0;c<nCols;c++)
		{
			T	accum = 0;
			for (size_t r=0;r<nRows;r++) accum += (*this)(r,c);
			outMeanVector[c] = accum / nRows;
		}

		MRPT_TRY_END;
	}

	/** Computes a row with the mean values of each column in the matrix and the associated vector with the standard deviation of each column.
	  * \sa mean,meanAndStdAll
	  */
	void  meanAndStd(
		std::vector<T> &outMeanVector,
		std::vector<T> &outStdVector )
	{
		MRPT_TRY_START;

		size_t	nCols = CMatrixTemplate<T>::getColCount();
		size_t	nRows = CMatrixTemplate<T>::getRowCount();
		ASSERT_(nCols>=1);
		ASSERT_(nRows>=1);

		// Compute the mean row:
		outMeanVector.resize(nCols);

		for (size_t c=0;c<nCols;c++)
		{
			T	accum = 0;
			for (size_t r=0;r<nRows;r++)
				accum += (*this)(r,c);
			outMeanVector[c] = accum / nRows;
		}

		// Compute the STD:
		outStdVector.resize(nCols);
		for (size_t c=0;c<nCols;c++)
		{
			T	accum = 0, thisMean = outMeanVector[c];
			for (size_t r=0;r<nRows;r++)
				accum += square( (*this)(r,c) - thisMean );
			if (nRows>1)
					outStdVector[c] = ::sqrt(accum / (nRows-1));
			else	outStdVector[c] = ::sqrt(accum / nRows);
		}

		MRPT_TRY_END;
	}

	/** Computes the mean and standard deviation of all the elements in the matrix as a whole.
	  * \sa mean,meanAndStd
	  */
	void  meanAndStdAll(
		T &outMean,
		T &outStd )
	{
		MRPT_TRY_START;

		size_t	nCols = CMatrixTemplate<T>::getColCount();
		size_t	nRows = CMatrixTemplate<T>::getRowCount();
		size_t	c;
		ASSERT_(nCols!=0);
		ASSERT_(nRows!=0);

		// Compute the mean:
		outMean=0;

		for (c=0;c<nCols;c++)
			for (size_t r=0;r<nRows;r++)
				outMean += (*this)(r,c);
		outMean /= nRows*nCols;

		// Compute the STD:
		outStd=0;
		for (c=0;c<nCols;c++)
			for (size_t r=0;r<nRows;r++)
				outStd += square( (*this)(r,c) - outMean );
		outStd = ::sqrt(outStd  / (nRows*nCols) );

		MRPT_TRY_END;
	}

	/** Extract a sub matrix from a matrix:
		* \exception std::exception On index out of bounds
		* The output matrix must have been previously set to the desired size.
		  * (by AJOGD @ JAN-2007)
		*/
	void  extractMatrix(size_t nRow, size_t nCol, CMatrixTemplateNumeric<T> &in)
	{
		size_t		i,j,ncols,nrows;

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

		for (i=nRow;i<nRow+nrows;i++)
			for(j=nCol;j<nCol+ncols;j++)
				in(i-nRow,j-nCol) = CMatrixTemplate<T>::m_Val[i][j];
	}

	/** Extract a sub matrix from a matrix:
	* \exception std::exception On index out of bounds
	* The output matrix must have been previously set to the desired size.
	* (by AJOGD @ JAN-2007)
	*/
	CMatrixTemplateNumeric<T>  extractMatrix(size_t nRow, size_t nCol,unsigned int nrows, unsigned int ncols)
	{
		unsigned int		i,j;
		CMatrixTemplateNumeric<T>	in(nrows,ncols);

		if ( (nRow+nrows > CMatrixTemplate<T>::getRowCount()) | (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
			THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");

		for (i=(unsigned int)nRow;i<(unsigned int)(nRow+nrows);i++)
			for(j=(unsigned int)nCol;j<(unsigned int)(nCol+ncols);j++)
				in(i-nRow,j-nCol) = CMatrixTemplate<T>::m_Val[i][j];
		return in;
	}

		/** Extract a sub matrix (vector) from a matrix:
		* \exception std::exception On index out of bounds
		* The output matrix must have been previously set to the desired size.
		  * (by AJOGD @ JAN-2007)
		*/
	void  extractMatrix(size_t nRow, size_t nCol, std::vector<T> &in)
	{
		size_t		j,ncols,nrows;

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

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

	/** Extract a sub matrix (vector) from a matrix:
	* \exception std::exception On index out of bounds
	* The output matrix must have been previously set to the desired size.
	* (by AJOGD @ JAN-2007)
	*/
	std::vector<T>  extractMatrix(size_t nRow, size_t nCol, unsigned int ncols)
	{
		size_t		j;
		std::vector<T> out;
		out.resize(ncols);

		if ( (nRow+1 > CMatrixTemplate<T>::getRowCount()) | (nCol+ncols >CMatrixTemplate<T>::getColCount()) )
			THROW_EXCEPTION("extractMatrix: Row or Col index out of bounds");

		for(j=nCol;j<nCol+ncols;j++)
				out[j-nCol] = CMatrixTemplate<T>::m_Val[nRow][j];
		return out;
	}

	void  asCol()
	{
		CMatrixTemplateNumeric<T>	aux;
		aux.setSize(CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows,1);

		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
			for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
				aux(j+i*CMatrixTemplate<T>::m_Cols,0)=CMatrixTemplate<T>::m_Val[i][j];
		(*this) = aux;
	}


	void  asRow()
	{
		CMatrixTemplateNumeric<T>	aux;
		aux.setSize(1,CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows);

		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
			for (size_t j=0;j<CMatrixTemplate<T>::m_Rows;j++)
				aux(0,j+i*CMatrixTemplate<T>::m_Cols)=CMatrixTemplate<T>::m_Val[i][j];
		(*this) = aux;
	}

	void  asCol(CMatrixTemplateNumeric<T>	&aux)
	{
		aux.setSize(CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows,1);

		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
			for (size_t j=0;j<CMatrixTemplate<T>::m_Cols;j++)
				aux(j+i*CMatrixTemplate<T>::m_Cols,0)=CMatrixTemplate<T>::m_Val[i][j];
	}


	void  asRow(CMatrixTemplateNumeric<T>	&aux)
	{
		aux.setSize(1,CMatrixTemplate<T>::m_Cols*CMatrixTemplate<T>::m_Rows);

		for (size_t i=0;i<CMatrixTemplate<T>::m_Rows;i++)
			for (size_t j=0;j<CMatrixTemplate<T>::m_Rows;j++)
				aux(0,j+i*CMatrixTemplate<T>::m_Cols)=CMatrixTemplate<T>::m_Val[i][j];
	}


	/** Finds elements whose values are a given number of times above (or below) the mean, in 1D Mahalanobis distance.
	  *  This returns two lists with the "row" and "column" indexes (i,j) of those elements m[i][j] such as:
	  *    m[i][j] > mean(matrix) + stdTimes·std(matrix)
	  *  The elements below the threshold
	  *    mean(matrix) - stdTimes·std(matrix)
	  *  can also be obtained setting "below" to "true".
	  */
	void  findElementsPassingMahalanobisThreshold(
		double					stdTimes,
		std::vector<size_t>		&rowIndexes,
		std::vector<size_t>		&colIndexes,
		bool					below = false )
	{
		MRPT_TRY_START;

		size_t	nCols = CMatrixTemplate<T>::getColCount();
		size_t	nRows = CMatrixTemplate<T>::getRowCount();

		rowIndexes.clear();
		colIndexes.clear();

		// Find mean and std:
		T	mean,std;
		meanAndStdAll(mean,std);

		// Compute threshold:
		double	thres = mean + stdTimes * std * (below ? (-1):1);

		if (below)
		{
			for (size_t c=0;c<nCols;c++)
				for (size_t r=0;r<nRows;r++)
					if ( (*this)(r,c) < thres )
					{
						rowIndexes.push_back(r);
						colIndexes.push_back(c);
					}
		}
		else
		{
			for (size_t c=0;c<nCols;c++)
				for (size_t r=0;r<nRows;r++)
					if ( (*this)(r,c) > thres )
					{
						rowIndexes.push_back(r);
						colIndexes.push_back(c);
					}
		}

		MRPT_TRY_END;
	}

	/** Adjusts the range of elements in the matrix such as the minimum and maximum values being those supplied by the user.
	  */
	void  adjustRange( T minVal=0,T maxVal=1)
	{
		MRPT_TRY_START;
		T	curMin = minimum();
		T	curMax = maximum();
		T	curRan = curMax-curMin;
		(*this) -= (curMin+minVal);
		if (curRan!=0) (*this) *= (maxVal-minVal)/curRan;
		MRPT_TRY_END;
	}

	/** Returns the sum of all the elements in the matrix
	  */
	T  sumAll()
	{
		MRPT_TRY_START;
		size_t	nCols = CMatrixTemplate<T>::getColCount();
		size_t	nRows = CMatrixTemplate<T>::getRowCount();
		T		accum=(T)0;

		for (size_t c=0;c<nCols;c++)
			for (size_t r=0;r<nRows;r++)
				accum += CMatrixTemplate<T>::m_Val[r][c];

		return accum;
		MRPT_TRY_END;
	}

	void  multiplyByMatrixAndByTrasposeNonSymmetric(
		const CMatrixTemplateNumeric<T>		&C,
		CMatrixTemplateNumeric<T>			&R)
	{
		MRPT_TRY_START;
		ASSERT_( (C.m_Rows == C.m_Cols) && (C.m_Rows == this->CMatrixTemplate<T>::m_Cols) );
		ASSERT_( &C != this );
		ASSERT_( &R != this );
		size_t						N = CMatrixTemplate<T>::m_Rows;
		size_t						M = CMatrixTemplate<T>::m_Cols;
		size_t						i,j,k,l;
		T							sumAccum,sumAccumInner;

		R.setSize(N,N);

		for (i=0;i<N;i++)
		{
			sumAccum = 0;
			for (l=0;l<M;l++)
			{
				sumAccumInner = 0;
				for (k=0;k<M;k++)
					sumAccumInner += CMatrixTemplate<T>::m_Val[i][k] * C.m_Val[k][l];
				for (j=0;j<N;j++)
					R.m_Val[i][j] += sumAccumInner * CMatrixTemplate<T>::m_Val[j][l];
			}
		}

		MRPT_TRY_END;
	}
/*************************************************AJOGD********************************************************/
	/**	Calculate the operation S = ABC where S is the object since we call this function
	*
	*/
	void  multiply3(
							const CMatrixTemplateNumeric<T>		&A,
							const CMatrixTemplateNumeric<T>		&B,
							const CMatrixTemplateNumeric<T>		&C)
	{
		MRPT_TRY_START;
		if ((A.m_Cols != B.m_Rows)||(B.m_Cols != C.m_Rows))
			THROW_EXCEPTION("Error in CMatrixTemplateNumeric. Wrong Matrix size in Multiply3");

		size_t						i,j,k,l;
		T							sumAccumInner;

		(*this).zeros(A.m_Rows,C.m_Cols);

		for (i=0;i<A.m_Rows;i++)
			for (l=0;l<B.m_Cols;l++)
			{
				sumAccumInner = 0;
				for (k=0;k<A.m_Cols;k++)
					sumAccumInner += A.m_Val[i][k] * B.m_Val[k][l];

				for (j=0;j<C.m_Cols;j++)
					CMatrixTemplate<T>::m_Val[i][j] += sumAccumInner * C.m_Val[l][j];
			}

		MRPT_TRY_END;
	}
/**************************************************************************************************************/

	/** This executes the operation \f$ \mathbf{R} = \mathbf{H} \mathbf{C} \mathbf{H}^t \f$, where 'this' matrix is \f$ \mathbf{H} \f$ and \f$ \mathbf{C} \f$ is symmetric, in an efficient and numerically stable way.
	  *  If 'this' matrix is \f$ N \times M \f$, then \f$ \mathbf{C} \f$ must be \f$ M \times M \f$, and the result matrix \f$ R \f$ will be \f$ N \times N \f$.
	  * The result from this method is assured to be symmetric (if \f$ \mathbf{C} \f$ is symmetric), whereas executing:
	  \code
	     R = H * C * (~H);
	  \endcode
	  * may lead to non-symmetric matrixes due to numerical rounding errors. In addition, this method is more efficient that the code above (see the MRPT's code examples on matrixes).
	  * \sa multiplyByMatrixAndByTrasposeScalar
	  */
	void  multiplyByMatrixAndByTraspose(
		const CMatrixTemplateNumeric<T>		&C,
		CMatrixTemplateNumeric<T>			&R)
	{
		MRPT_TRY_START;
		ASSERT_( (C.m_Rows == C.m_Cols) && (C.m_Rows == this->CMatrixTemplate<T>::m_Cols) );
		ASSERT_( &C != this );
		ASSERT_( &R != this );
		size_t						N = CMatrixTemplate<T>::m_Rows;
		size_t						M = CMatrixTemplate<T>::m_Cols;
		size_t						i,j,k,l;
		T							sumAccum,sumAccumInner;

		R.setSize(N,N);

		if (N*M>100)
		{
			CMatrixTemplateNumeric<T>	R_(N,M);

			// First compute R_ = this * C:
			for (i=0;i<N;i++)
				for (j=i;j<M;j++)
				{
					sumAccum = 0;
					for (l=0;l<M;l++)
						sumAccum += CMatrixTemplate<T>::m_Val[i][l]*C.m_Val[l][j];
					R_.m_Val[i][j] = sumAccum;
				}

			// Now compute R = R_ * (~this):
			for (i=0;i<N;i++)
				for (j=i;j<M;j++)
				{
					sumAccum = 0;
					for (l=0;l<M;l++)
						sumAccum += R_.m_Val[i][j] * CMatrixTemplate<T>::m_Val[j][l];
					R.m_Val[i][j] = R.m_Val[j][i] = sumAccum;
				}
		}
		else
		{
			// This method is faster for small matrixes:

			for (i=0;i<N;i++)
			{
				for (j=i;j<N;j++)
				{
					sumAccum = 0;
					for (l=0;l<M;l++)
					{
						sumAccumInner = 0;
						for (k=0;k<M;k++)
							sumAccumInner += CMatrixTemplate<T>::m_Val[i][k] * C.m_Val[k][l];
						sumAccum += sumAccumInner * CMatrixTemplate<T>::m_Val[j][l];
					}
					R.m_Val[i][j] = R.m_Val[j][i] = sumAccum;
				}
			}
		}

		MRPT_TRY_END;
	}

	/** An special case of multiplyByMatrixAndByTraspose for the case of the resulting matrix being a scalar (that is, a 1x1 matrix) - This method directly returns this as a scalar avoiding the construction of a 1x1 matrix.
	  *  This matrix (H) must be \f$ 1 \times N \f$ or a \f$ N \times 1 \f$ matrix and C must by \f$ N \times N \f$, or an exception will be raised.
	  *  Refer to multiplyByMatrixAndByTraspose for more information.
	  \code
	     return = ( H * C * (~H) ) (0,0);
	  \endcode
	  * \sa multiplyByMatrixAndByTraspose
	  */
	T  multiplyByMatrixAndByTrasposeScalar(
		const CMatrixTemplateNumeric<T>		&C )
	{
		MRPT_TRY_START;
		ASSERT_( (C.m_Rows == C.m_Cols) );
		ASSERT_( &C != this );
		size_t						N = CMatrixTemplate<T>::m_Rows;
		size_t						M = CMatrixTemplate<T>::m_Cols;
		size_t						k,l;
		T							sumAccum,sumAccumInner;

		if (N==1)
		{
			// This is 1xM matrix, C is a MxM matrix
			ASSERT_( C.m_Rows == M );

			sumAccum = 0;
			for (l=0;l<M;l++)
			{
				sumAccumInner = 0;
				for (k=0;k<M;k++)
					sumAccumInner += CMatrixTemplate<T>::m_Val[0][k] * C.m_Val[k][l];
				sumAccum += sumAccumInner * CMatrixTemplate<T>::m_Val[0][l];
			}
			return sumAccum;

		}
		else
		if (M==1)
		{
			// This is Nx1 matrix, C is a NxN matrix:
			// Assume the user really means (~H)*C*H instead of H*C*(~H):
			ASSERT_( C.m_Rows == N );

			sumAccum = 0;
			for (l=0;l<N;l++)
			{
				sumAccumInner = 0;
				for (k=0;k<N;k++)
					sumAccumInner += CMatrixTemplate<T>::m_Val[k][0] * C.m_Val[k][l];
				sumAccum += sumAccumInner * CMatrixTemplate<T>::m_Val[l][0];
			}
			return sumAccum;
		}
		else THROW_EXCEPTION("This matrix must be a one-column or a one-row matrix!");


		MRPT_TRY_END;
	}


private:
	/** Internal use.
	  */
	int pivot(size_t row)
	{
		size_t k = row;
		double amax,temp;

		amax = -1;
		for (size_t i=row; i < CMatrixTemplate<T>::m_Rows; i++)
			if ( (temp = fabs( CMatrixTemplate<T>::m_Val[i][row])) > amax && temp != (T)0)
			{
				amax = temp;
				k = i;
			}
		if (CMatrixTemplate<T>::m_Val[k][row] == T(0))
			return -1;
		if (k != row)
		{
			T* rowptr = CMatrixTemplate<T>::m_Val[k];
			CMatrixTemplate<T>::m_Val[k] = CMatrixTemplate<T>::m_Val[row];
			CMatrixTemplate<T>::m_Val[row] = rowptr;
			return (int)k;
		}
		return 0;
	}

	/**
		Householder reduction of a real, symmetric matrix a[1..n][1..n]. On output, a is replaced
		by the orthogonal matrix Q reflecting the transformation. d[1..n] returns the diagonal elements
		of the tridiagonal matrix, and e[1..n] the off-diagonal elements, with e[1]=0. Several
		statements, as noted in comments, can be omitted if only eigenvalues are to be found, in which
		case a contains no useful information on output. Otherwise they are to be included.
	*/
	static void  tred2(T **a, size_t n, T d[], T e[])
	{
		int		l,k,j,i;
		T		scale,hh,h,g,f;

		for (i=(int)n;i>=2;i--)
		{
			l=i-1;
			h=scale=0.0;
			if (l > 1)
			{
				for (k=1;k<=l;k++)
				scale += fabs(a[i][k]);
				if (scale == 0)				// Skip transformation.
					e[i]=a[i][l];
				else
				{
					for (k=1;k<=l;k++)
					{
						a[i][k] /= scale;		// Use scaled a's for transformation.
						h += a[i][k]*a[i][k];	// Form sigma in h.
					}
					f=a[i][l];
					g=(f >= 0 ? -::sqrt(h) : (::sqrt(h)));
					e[i]=scale*g;
					h -= f*g;						// Now h is equation (11.2.4).
					a[i][l]=f-g;					// Store u in the ith row of a.
					f=0;
					for (j=1;j<=l;j++)
					{
						a[j][i]=a[i][j]/h;			// Store u=H in ith column of a.
						g=0.0;						// Form an element of A . u in g.
						for (k=1;k<=j;k++)
							g += a[j][k]*a[i][k];
						for (k=j+1;k<=l;k++)
							g += a[k][j]*a[i][k];
						e[j]=g/h;					// Form element of p in temporarily unused element of e.
						f += e[j]*a[i][j];
					}
					hh=f/(h+h);						// Form K, equation (11.2.11).
					for (j=1;j<=l;j++)
					{								// Form q and store in e overwriting p.
						f=a[i][j];
						e[j]=g=e[j]-hh*f;
						for (k=1;k<=j;k++)			// Reduce a, equation (11.2.13).
							a[j][k] -= (f*e[k]+g*a[i][k]);
					}
				}
			} else
				e[i]=a[i][l];
			d[i]=h;
		}

		/* Next statement can be omitted if eigenvectors not wanted */
		d[1]=0;
		e[1]=0;

		/* Contents of this loop can be omitted if eigenvectors not
		wanted except for statement d[i]=a[i][i]; */
		for (i=1;i<=(int)n;i++)	// Begin accumulation of transformationmatrices
		{
			l= i-1;
			if (d[i]) // This block skipped when i=1.
				for (j=1;j<=l;j++)
				{
					g=0.0;
					for (k=1;k<=l;k++) // Use u and u=H stored in a to form P.Q.
						g += a[i][k]*a[k][j];
					for (k=1;k<=l;k++)
						a[k][j] -= g*a[k][i];
				}
			d[i]=a[i][i];			// This statement remains.
			a[i][i]=1;				// Reset row and column of a to identity
			for (j=1;j<=l;j++)
				a[j][i]=a[i][j]= 0; // matrix for next iteration.
		}
	}

	/** QL algorithm with implicit shifts, to determine the eigenvalues and eigenvectors of a real, symmetric,
		tridiagonal matrix, or of a real, symmetric matrix previously reduced by tred2 x11.2. On
		input, d[1..n] contains the diagonal elements of the tridiagonal matrix. On output, it returns
		the eigenvalues. The vector e[1..n] inputs the subdiagonal elements of the tridiagonal matrix,
		with e[1] arbitrary. On output e is destroyed. When finding only the eigenvalues, several lines
		may be omitted, as noted in the comments. If the eigenvectors of a tridiagonal matrix are desired,
		the matrix z[1..n][1..n] is input as the identity matrix. If the eigenvectors of a matrix
		that has been reduced by tred2 are required, then z is input as the matrix output by tred2.
		In either case, the kth column of z returns the normalized eigenvector corresponding to d[k].
	*/
	static void  tqli(T d[], T e[], size_t n, T **z)
	{
		int			m,l,iter,i,k;
		T			s,r,p,g,f,dd,c,b;
		T volatile	temp;

		for (i=2;i<=(int)n;i++)
			e[i-1]=e[i];				// Convenient to renumber the elements of e.

		e[n]=0.0;
		for (l=1;l<=(int)n;l++)
		{
			iter=0;
			do
			{
				for (m=l;m<=(int)(n-1);m++)	// Look for a single small subdiagonal element to split the matrix.
				{
					dd=(T)( fabs(d[m])+fabs(d[m+1]) );
					// ----------------------------------------------------------------------
					// Fix added 14/DEC/2006: Avoid a convergence problem due to compiler
					//  optimization. See http://www.nr.com/forum/showthread.php?p=1803
					// ----------------------------------------------------------------------
					temp = fabs(e[m])+dd;
					if (temp == dd) break;
				}
				if (m != l)
				{
					if (iter++ == 60) THROW_EXCEPTION("tqli: Too many iterations in tqli!")

					g=(T)( (d[l+1]-d[l])/(2.0*e[l]));			// Form shift.
					r=pythag(g,1.0);
					g=d[m]-d[l]+e[l]/(g+SIGN(r,g));		// This is dm - ks.
					s = c = 1;
					p = 0;
					for (i=m-1;i>=l;i--)				// A plane rotation as in the original QL, followed by Givens rotations to restore tridiagonal	form.
					{
						f=s*e[i];
						b=c*e[i];
						e[i+1]=(r=pythag(f,g));
						if (r == 0.0)						// Recover from underflow.
						{
							d[i+1] -= p;
							e[m]=0.0;
							break;
						}
						s=f/r;
						c=g/r;
						g=d[i+1]-p;
						r=(d[i]-g)*s+2.0f*c*b;
						d[i+1]=g+(p=s*r);
						g=c*r-b;
						/* Next loop can be omitted if eigenvectors not wanted*/
						for (k=1;k<=(int)n;k++)  // Form eigenvectors.
						{
							f=z[k][i+1];
							z[k][i+1]=s*z[k][i]+c*f;
							z[k][i]=c*z[k][i]-s*f;
						}
					}

					if (r == 0.0 && i >= l) continue;
					d[l] -= p;
					e[l]=g;
					e[m]=0.0;
				}
			} while (m != l);
		}
	}

	/** Auxiliary function (internal use only)
	  */
	static T  pythag(T a, T b)
	{
		static T at, bt, ct;
		return (T)( ((at = fabs(a)) > (bt = fabs(b)) ?
				(ct = bt/at, at*(::sqrt(1.0+ct*ct))) :
				(bt ? (ct = at/bt, bt*(::sqrt(1.0+ct*ct))): 0)) );
	}

	/** Auxiliary function (internal use only)
	  */
	static T  SIGN(T a,T b)
	{
		return (T)( ((b) >= 0 ? fabs(a) : -fabs(a)) );
	}

	/** SVD-Decomposition.
	 *   Extracted from "Numerical recipes in C++" eBook at http://www.nr.com<br>
	  */
	static void  svdcmp(T* a[], int m, int n, T w[], T* v[])
	{
		// Given a matrix a[m][n], this routine computes its singular value
		// decomposition, A = U*W*V'.  The matrix U replaces a on output.
		// The diagonal matrix of singular values W is output as a vector w[n].
		// The matrix V  is output as v[n][n].
		// m must be greater or equal to n;  if it is smaller, then a should be
		// filled up to square with zero rows.

		int flag, i, its, j, jj, k, l, nm;
		double c, f, h, s, x, y, z;
		double anorm = 0.0, g = 0.0, scale = 0.0;

		if (m < n) THROW_EXCEPTION("svdcmp(): Matrix is not augmented with extra rows of zeros");
		std::vector<double> rv1(n);

		// Householder reduction to bidiagonal form.
		l = 0;    // added by T. Wang to avoid warning in g++
		nm = 0;   // added by T. Wang to avoid warning in g++
		for (i = 0; i < n; i++)
		{
			l = i + 1;
			rv1[i] = scale*g;
			g = s = scale = 0.0;
			if (i < m) {
				for (k = i; k < m; k++) scale += fabs(a[k][i]);
					if (scale) {
					for (k = i; k < m; k++) {
						a[k][i] /= (T)scale;
						s += a[k][i]*a[k][i];
					}
					f = a[i][i];
					g = -SIGN(::sqrt((T)s),(T)f);
					h = f*g - s;
					a[i][i] = (T)(f - g);
					if (i != n - 1) {
						for (j = l; j < n; j++) {
							for (s  = 0.0, k = i; k < m; k++) s += a[k][i]*a[k][j];
							f = s/h;
							for ( k = i; k < m; k++) a[k][j] += (T)(f*a[k][i]);
						}
					}
					for (k = i; k < m; k++) a[k][i] *= (T)scale;
					}
				}
				w[i] = (T)(scale*g);
				g = s= scale = 0.0;
				if (i < m && i != n - 1) {
					for (k = l; k < n; k++)  scale += fabs(a[i][k]);
					if (scale) {
					for (k = l; k < n; k++) {
						a[i][k] /= (T)scale;
						s += a[i][k]*a[i][k];
					}
					f = a[i][l];
					g = -SIGN(::sqrt((T)s), (T)f);
					h = f*g - s;
					a[i][l] = (T)(f - g);
					for (k = l; k < n; k++)  rv1[k] = a[i][k]/h;
					if (i != m - 1) {
						for (j = l; j < m; j++) {
							for (s = 0.0, k = l; k < n; k++) s += a[j][k]*a[i][k];
							for (k = l; k < n; k++) a[j][k] += (T)(s*rv1[k]);
						}
					}
					for (k = l; k < n; k++) a[i][k] *= (T)scale;
					}
				}
				anorm = max(anorm, (fabs(w[i]) + fabs(rv1[i])));
			}
				/* Accumulation of right-hand transformations.  */
			for (i = n - 1; 0 <= i; i--) {
				if (i < n - 1) {
					if (g) {
					for (j = l; j < n; j++)  v[j][i] = (T)((a[i][j]/a[i][l])/g);
						/* Double division to avoid possible underflow: */
					for (j = l; j < n; j++) {
						for (s = 0.0, k = l; k < n; k++) s += a[i][k]*v[k][j];
						for (k = l; k < n; k++)  v[k][j] += (T)(s*v[k][i]);
					}
					}
					for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
		}
				v[i][i] = 1.0;
				g = rv1[i];
				l = i;
			}
				/* Accumulation of left-hand transformations.   */
			for (i = n - 1; 0 <= i; i--) {
				l = i + 1;
				g = w[i];
				if (i < n - 1) for (j = l; j < n; j++) a[i][j] = 0.0;
				if (g) {
					g = 1.0/g;
					if (i != n - 1) {
					for (j = l; j < n; j++) {
						for (s = 0.0, k = l; k < m; k++) s += a[k][i]*a[k][j];
						f = (s/a[i][i])*g;
						for (k = i; k < m; k++) a[k][j] += (T)(f*a[k][i]);
					}
					}
					for (j = i; j < m; j++)  a[j][i] *= (T)g;
				}
				else {
					for (j = i; j < m; j++) a[j][i] = 0.0;
				}
				a[i][i] += 1.0;   // ++a[i][i]
			}
			/* Diagonalization of the bidiagonal form.  */
			for (k = n - 1; 0 <= k; k--) {        /* Loop over singular values. */
				for (its = 0; its < 30; its++) {    /* Loop over allowed iterations.*/
					flag = 1;
					for (l = k; 0 <= l; l--) {     // Test for splitting:
					nm = l - 1;                 // Note that rv1[0] is always zero
					if (fabs(rv1[l]) + anorm == anorm) {
						flag = 0;
						break;
					}
					if ( fabs(w[nm]) + anorm == anorm) break;
					}
					if (flag) {
					c = 0.0;                       /* Cancellation of rv1[l], if l>0:*/
					s = 1.0;
					for (i = l; i <= k; i++) {
					f = s*rv1[i];
					if (fabs(f) + anorm != anorm) {
						g = w[i];
						h = pythag((T)f, (T)g);
						w[i] = (T)h;
						h = (T)(1.0/h);
						c = (T)(g*h);
						s = (-f*h);
						for (j = 0; j < m; j++) {
							y = a[j][nm];
							z = a[j][i];
							a[j][nm] = (T)(y*c + z*s);
							a[j][i]  = (T)(z*c - y*s);
						}
					}
					}
				}
				z = w[k];
				if (l == k) {       /* Convergence.  */
					if (z < 0.0) {        /* Singular value is made non-negative. */
					w[k] = (T)-z;
					for (j = 0; j < n; j++) v[j][k] = (-v[j][k]);
					}
					break;
				}
				if (its == 29)
				THROW_EXCEPTION("svdcmp(): Not convergence in 30 SVDCMP iterations!");

				x = w[l];               /* Shift from bottom 2-by-2 minor. */
				nm = k - 1;
				y = w[nm];
				g = rv1[nm];
				h = rv1[k];
				f = ((y - z)*(y + z) + (g - h)*(g + h))/(2.0*h*y);
				g = pythag((T)f, (T)1.0);
				f = ((x - z)*(x + z) + h*((y/(f + SIGN((T)g, (T)f))) - h))/x;
					/* Next QR transformation:    */
				c = s = 1.0;
				for (j = l; j <= nm; j++) {
					i = j + 1;
					g = rv1[i];
					y = w[i];
					h = s*g;
					g = c*g;
					z = pythag((T)f, (T)h);
					rv1[j] = z;
					c = f/z;
					s = h/z;
					f = x*c + g*s;
					g = g*c - x*s;
					h = y*s;
					y = y*c;
					for (jj = 0; jj < n;  jj++) {
					x = v[jj][j];
					z = v[jj][i];
					v[jj][j] = (T)(x*c + z*s);
					v[jj][i] = (T)(z*c - x*s);
					}
					z = pythag((T)f, (T)h);
					w[j] = (T)z;        /* Rotation can be arbitrary if z = 0.*/
					if (z) {
					z = 1.0/z;
					c = f*z;
					s = h*z;
					}
					f = (c*g) + (s*y);
					x = (c*y) - (s*g);
					for (jj = 0; jj < m; jj++) {
					y = a[jj][j];
					z = a[jj][i];
					a[jj][j] = (T)(y*c + z*s);
					a[jj][i] = (T)(z*c - y*s);
					}
				}
				rv1[l] = 0.0;
				rv1[k] = f;
				w[k] = (T)x;
			}
		}
	}


}; // end of class definition


/** Logical equal-to operator
 */
template <class T>
bool  operator == (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
{
   if (m1.getRowCount() != m2.getRowCount() || m1.getColCount() != m2.getColCount())
      return false;

   for (size_t i=0; i < m1.getRowCount(); i++)
      for (size_t j=0; j < m1.getColCount(); j++)
	      if (m1(i,j) != m2(i,j))
	         return false;

   return true;
}

/** logical no-equal-to operator
 */
template <class T>
bool  operator != (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
{
    return !(m1 == m2);
}

/** binary addition operator
 */
template <class T>
CMatrixTemplateNumeric<T>  operator + (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
{
   CMatrixTemplateNumeric<T>	temp(m1);
   temp += m2;
   return temp;
}

/** binary subtraction operator
 */
template <class T>
 CMatrixTemplateNumeric<T>  operator - (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
{
   CMatrixTemplateNumeric<T>	temp(m1);
   temp -= m2;
   return temp;
}

/** binary scalar multiplication operator
 */
template <class T>
CMatrixTemplateNumeric<T>  operator * (const CMatrixTemplateNumeric<T>& m, const T& no)
{
	CMatrixTemplateNumeric<T>	temp(m);
	temp*=no;
	return temp;
}

/** binary scalar multiplication operator
 */
template <class T>
CMatrixTemplateNumeric<T>  operator * (const T& no, const CMatrixTemplateNumeric<T>& m)
{
	CMatrixTemplateNumeric<T>	temp(m);
	temp*=no;
	return temp;
}

/** binary matrix multiplication operator
 */
template <class T>
CMatrixTemplateNumeric<T>  operator * (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
{
	CMatrixTemplateNumeric<T>	res(m1);
	res*=m2;
	return res;
}

/** binary scalar division operator
 */
template <class T>
 CMatrixTemplateNumeric<T>  operator / (const CMatrixTemplateNumeric<T>& m, const T& no)
{
	return (m * (T(1) / no));
}

/** binary scalar division operator
 */
template <class T>
 CMatrixTemplateNumeric<T>  operator / (const T& no, const CMatrixTemplateNumeric<T>& m)
{
	return (!m * no);
}

/** binary matrix division operator
 */
template <class T>
 CMatrixTemplateNumeric<T>  operator / (const CMatrixTemplateNumeric<T>& m1, const CMatrixTemplateNumeric<T>& m2)
{
	return (m1 * !m2);
}

/** binary power operator
 */
template <class T>
 CMatrixTemplateNumeric<T>  operator ^ (const CMatrixTemplateNumeric<T>& m, const unsigned int& pow)
{
	CMatrixTemplateNumeric<T>	temp(m);
	temp ^= pow;
	return temp;
}

/** unary transpose operator
 */
template <class T>
CMatrixTemplateNumeric<T>  operator ~ (const CMatrixTemplateNumeric<T>& m)
{
	CMatrixTemplateNumeric<T>	temp(m.getColCount(),m.getRowCount());

	for (size_t i=0; i < m.getRowCount(); i++)
		for (size_t j=0; j < m.getColCount(); j++)
		{
			T	x = m(i,j);
			temp(j,i) = x;
		}
	return temp;
}

/** Unary inversion operator.
 */
template <class T>
CMatrixTemplateNumeric<T>  operator ! (const CMatrixTemplateNumeric<T> &m)
{
	return m.inv();
}

/** A useful macro for saving matrixes to a file while debugging.
  */
#define DEBUG_SAVE_MATRIX(M) \
	{ \
		char auxStr[100]; \
		MRPT_OS::sprintf(auxStr,99,"%s.txt",#M); \
		M.saveToTextFile(auxStr); \
	} \


} // End of namespace



#endif //__STD_MATRIX_H
