/*---------------------------------------------------------------
	FILE: MATH.h
	USE: See description below.

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

#include <MRPT/UTILS/utils_defs.h>
#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CVectorTemplate.h>

/*---------------------------------------------------------------
		Namespace
  ---------------------------------------------------------------*/
namespace UTILS
{
	/** This base provides a set of functions for maths stuff.
	 */
	namespace MATH
	{
	    /** Returns true if value is Not-a-number (NAN)
	      */
	    bool isNan(float v);

	    /** Returns true if value is Not-a-number (NAN)
	      */
	    bool isNan(double v);

	    /** Returns true if value is finite
	      */
	    bool isFinite(float v);

	    /** Returns true if value is finite
	      */
	    bool isFinite(double v);

		/** Evaluates the univariate normal (Gaussian) distribution at a given point "x".
		  */
		double  normalPDF(double x, double mu, double std);

		/** Evaluates the multivariate normal (Gaussian) distribution at a given point "x" ("x" and "mu" can be 1xN or Nx1 matrixes).
		  */
		template <class T>
		T  normalPDF(
			const CMatrixTemplateNumeric<T>		&x,
			const CMatrixTemplateNumeric<T>		&mu,
			const CMatrixTemplateNumeric<T>		&cov)
		{
			MRPT_TRY_START;

			CMatrixTemplateNumeric<T>	S = cov.inv();
			size_t						d = cov.getRowCount();		// Dimension:
			CMatrixTemplateNumeric<T>	X, MU, X_MU;

			if (x.getRowCount()==1)
					X = ~x;
			else	X = x;

			if (mu.getRowCount()==1)
					MU = ~mu;
			else	MU = mu;

			X_MU = X - MU;

			return ::exp( (T)(-0.5)  * ( (~X_MU)*S*X_MU )(0,0) ) / (::pow((T)M_2PI,(T)d) * ::sqrt(cov.det()));

			MRPT_TRY_END;
		}

		/** The complementary error function of a Normal distribution
		  */
		double  erfc(double x);

		/** The error function of a Normal distribution
		  */
		double  erf(double x);

		/** Evaluates the Gaussian distribution quantile for the probability value p=[0,1].
		  *  The employed approximation is that from Peter J. Acklam (pjacklam@online.no),
		  *  freely available in http://home.online.no/~pjacklam.
		  */
		double  normalQuantile(double p);

		/** Evaluates the Gaussian cumulative density function.
		  *  The employed approximation is that from W. J. Cody
		  *  freely available in http://www.netlib.org/specfun/erf
		  */
		double  normalCDF(double p);

		/** The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1
		  * An aproximation from the Wilson-Hilferty transformation is used.
		  */
		double  chiSquareQuantile(double P, unsigned int dim=1);

		/** Finds the maximum value (and the corresponding zero-based index) from a given vector.
		  */
		template<class T>
		T  maximum(const std::vector<T> &v, unsigned int *maxIndex = NULL)
		{
			T							M=0;
			int							i,M_idx=-1;
			typename std::vector<T>::const_iterator	it;
			for (i=0, it=v.begin(); it!=v.end();it++,i++)
				if (*it>M || M_idx==-1)
				{
					M = *it;
					M_idx = i;
				}
			if (maxIndex) *maxIndex = M_idx;
			return M;
		}

		/** Finds the maximum value (and the corresponding zero-based index) from a given vector.
		  */
		template <class T>
		T  minimum(const std::vector<T> &v, unsigned int *minIndex = NULL)
		{
			T							M=0;
			int							i,M_idx=-1;
			typename std::vector<T>::const_iterator	it;
			for (i=0, it=v.begin(); it!=v.end();it++,i++)
				if (*it<M || M_idx==-1)
				{
					M = *it;
					M_idx = i;
				}
			if (minIndex) *minIndex = M_idx;
			return M;
		}

		/** Computes the mean value of a vector
		  * \sa MATH::std,MATH::meanAndStd
		  */
		template<class T>
			double  mean(const std::vector<T> &v)
		{
			if (v.size()==0)
				return 0;
			else
			{
				double	m = 0;
				for (typename std::vector<T>::const_iterator it = v.begin();it!=v.end();it++) m += (*it);
				return ((double)( m / v.size() ));
			}
		}

		/** Computes the sum of all the elements of a vector
		  * \sa cumsum
		  */
		template<class T>
			T  sum(const std::vector<T> &v)
		{
			T	 ret = (T)0;
			for (typename std::vector<T>::const_iterator it = v.begin();it!=v.end();it++) ret += (*it);
			return ret;
		}

		/** Computes the cumulative sum of all the elements of a vector
		  * \sa sum
		  */
		template<class T>
			std::vector<T>  cumsum(const std::vector<T> &v)
		{
			std::vector<T>	 ret(v.size(),0);
			T			     last = 0;
			for (typename std::vector<T>::const_iterator it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++)
				last = (*it2) = last + (*it);
			return ret;
		}


		/** Computes the standard deviation of a vector
		  * \param v The set of data
		  * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
		  * \sa MATH::mean,MATH::meanAndStd
		  */
		template<class T>
			double  std(const std::vector<T> &v, bool unbiased = true)
		{
			if (v.size()<2)
				return 0;
			else
			{
				// Compute the mean:
				typename std::vector<T>::const_iterator it;
				double					 vector_std=0,vector_mean = 0;
				for (it = v.begin();it!=v.end();it++) vector_mean += (*it);
				vector_mean /= (double)v.size();
				// Compute the std:
				for (it = v.begin();it!=v.end();it++) vector_std += square((*it)-vector_mean);
				vector_std = sqrt(vector_std  / (double)(v.size() - (unbiased ? 1:0)) );

				return vector_std;
			}
		}

		/** Computes the standard deviation of a vector
		  * \param v The set of data
		  * \param out_mean The output for the estimated mean
		  * \param out_std The output for the estimated standard deviation
		  * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
		  * \sa MATH::mean,MATH::std
		  */
		template<class T>
			void  meanAndStd(
				const std::vector<T>	&v,
				double			&out_mean,
				double			&out_std,
				bool			unbiased = true)
		{
			if (v.size()<2)
			{
				out_std = 0;
				if (v.size()==1)
					out_mean = v[0];
			}
			else
			{
				// Compute the mean:
				typename std::vector<T>::const_iterator it;
				out_std=0,out_mean = 0;
				for (it = v.begin();it!=v.end();it++) out_mean += (*it);
				out_mean /= (double)v.size();

				// Compute the std:
				for (it = v.begin();it!=v.end();it++) out_std += square(((double)(*it))-out_mean);
				out_std = sqrt(out_std / (double)(v.size() - (unbiased ? 1:0)) );
			}
		}

		/** Computes the weighted histogram for a vector of values and their corresponding weights.
		  *  \param values [IN] The N values
		  *  \param weights [IN] The weights for the corresponding N values
		  *  \param binWidth [IN] The desired width of the bins
		  *  \param out_binCenters [OUT] The centers of the M bins generated to cover from the minimum to the maximum value of "values" with the given "binWidth"
		  *  \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
		  */
		template<class T>
			void  weightedHistogram(
				const std::vector<T>		&values,
				const std::vector<T>		&weights,
				float				binWidth,
				std::vector<float>	&out_binCenters,
				std::vector<float>	&out_binValues )
			{
				MRPT_TRY_START;

				ASSERT_( values.size() == weights.size() );
				ASSERT_( binWidth > 0 );
				T				minBin = minimum( values );
				unsigned int	nBins = (unsigned int)ceil((maximum( values )-minBin) / binWidth);

				// Generate bin center and border values:
				out_binCenters.resize(nBins);
				out_binValues.clear(); out_binValues.resize(nBins,0);
				float		   halfBin = 0.5f*binWidth;;
				vector_float   binBorders(nBins+1,minBin-halfBin);
				for (unsigned int i=0;i<nBins;i++)
				{
					binBorders[i+1] = binBorders[i]+binWidth;
					out_binCenters[i] = binBorders[i]+halfBin;
				}

				// Compute the histogram:
				float	totalSum = 0;
				for (typename  std::vector<T>::const_iterator itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); itVal++, itW++ )
				{
					unsigned int idx = (unsigned int)round(((*itVal)-minBin)/binWidth);
					if (idx>=nBins) idx=nBins-1;
					ASSERT_(idx>=0);
					out_binValues[idx] += *itW;
					totalSum+= *itW;
				}

				if (totalSum)
					out_binValues = out_binValues / totalSum;


				MRPT_TRY_END;
			}


		/** Computes the factorial of an integer number and returns it as a 64-bit integer number.
		  */
		uint64_t  factorial64(unsigned int n);

		/** Computes the factorial of an integer number and returns it as a double value (internally it uses logarithms for avoiding overflow).
		  */
		double  factorial(unsigned int n);

		/** Returns the given angle translated into the ]-pi,pi] range (only for values in the range [-3pi,3pi])
		  */
		double  angleTo2PIRange(double a);

		/** Returns the given angle translated into the ]-pi,pi] range (only for values in the range [-3pi,3pi])
		  */
		float  angleTo2PIRange(float a);

		/** Round up to the nearest power of two of a given number
		  */
		template <class T>
		T round2up(T val)
		{
			T n = 1;
			while (n < val)
			{
				n <<= 1;
				if (n<=1)
					THROW_EXCEPTION("Overflow!");
			}
			return n;
			}

		/** Computes the FFT of a 2^N-size vector of real numbers, and returns the Re+Im+Magnitude parts.
		  * \sa fft2_real
		  */
		void  fft_real(	vector_float	&in_realData,
								vector_float	&out_FFT_Re,
								vector_float	&out_FFT_Im,
								vector_float	&out_FFT_Mag );

		/** Cholesky factorization: in = out' · out
		  *	Given a positive-definite symmetric matrix, this routine constructs its Cholesky decomposition.
		  * On input, only the upper triangle of "IN" need be given; it is not modified.
		  *  The Cholesky factorization is returned in "out" in the upper triangle.
		  *  (by AJOGD @ JAN-2007)
		  */
		template<class T>
		void  chol(const CMatrixTemplateNumeric<T> &in,CMatrixTemplateNumeric<T> &out)
		{
			if (in.getColCount() != in.getRowCount()) THROW_EXCEPTION("Cholesky factorization error, in matrix not square");
			size_t	i,j,k;
			T		sum;
			out.setSize(in.getRowCount(),in.getColCount());
			for (i=0;i<in.getRowCount();i++)
			{
				for (j=i;j<in.getColCount();j++)
				{
					sum=in(i,j);
					for (k=i-1;(k>=0)&(k<in.getColCount());k--)
					{
						sum -= out(k,i)*out(k,j);
					}
					if (i==j)
					{
						if (sum<0)
						{
							in.saveToTextFile("c:\\cholesky_non_defined_positive.txt");
							THROW_EXCEPTION("Cholesky factorization error, in matrix not defined-positive");
						}
						out(i,j)=sqrt(sum);
					}
					else
					{
						out(i,j)=sum/out(i,i);
						out(j,i)=0;
					}
				}
			}
		}

		/** Calculate the correlation between two matrices
		  *  (by AJOGD @ JAN-2007)
		  */
		template<class T>
		double  correlate_matrix(const CMatrixTemplateNumeric<T> &a1, const CMatrixTemplateNumeric<T> &a2)
		{
			if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
				THROW_EXCEPTION("Correlation Error!, images with no same size");

			int i,j;
			T x1,x2;
			T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();

			//find the means
			for (i=0;i<a1.getRowCount();i++)
			{
				for (j=0;j<a1.getColCount();j++)
				{
					m1 += a1(i,j);
					m2 += a2(i,j);
				}
			}
			m1 /= n;
			m2 /= n;

			for (i=0;i<a1.getRowCount();i++)
			{
				for (j=0;j<a1.getColCount();j++)
				{
					x1 = a1(i,j) - m1;
					x2 = a2(i,j) - m2;
					sxx += x1*x1;
					syy += x2*x2;
					sxy += x1*x2;
				}
			}

			return sxy / sqrt(sxx * syy);
		}


		/**	Matrix QR decomposition. A = QR, where R is upper triangular and Q is orthogonal, that is, ~QQ = 1
		  * If A is a LxM dimension matrix, this function only return the LxL upper triangular matrix R instead of LxM pseudo-upper
		  * triangular matrix (been L<=M)
		  * This function has been extracted from "Numerical Recipes in C".
		  *	/param A is the original matrix to decompose
		  * /param c,Q. The orthogonal matrix Q is represented as a product of n-1 Householder matrices Q1,...Qn-1, where Qj = 1 - u[j] x u[j]/c[j]
		  *				The i'th component of u[j] is zero for i = 1,...,j-1 while the nonzero components are returned in Q(i,j) for i=j,...,n
		  *	/param R is the upper triangular matrix
		  *	/param sign returns as true (1) is singularity is encountered during the decomposition, but the decomposition is still complete
		  *				in this case; otherwise it returns false (0)
		  */

			template<class T>
			void	qr_decomposition(
				CMatrixTemplateNumeric<T>	&A,
				CMatrixTemplateNumeric<T>	&R,
				CMatrixTemplateNumeric<T>	&Q,
				CVectorTemplate<T>			&c,
				int								&sing);


		/**If R = CHOL(A) is the original Cholesky factorization of A, then R1 = CHOLUPDATE(R,X) returns the upper triangular
		  * Cholesky factor of A + X*X', where X is a column vector of appropriate length.
		  */

			template<class T>
			void UpdateCholesky(
				CMatrixTemplateNumeric<T>	&chol,
				CVectorTemplate<T>			&r1Modification);


		/** Compute the 2D Discrete Fourier Transform (DFT) of a real matrix, returning the real and imaginary parts separately.
		  * \param in_data The N_1xN_2 matrix.
		  * \param out_real The N_1xN_2 output matrix which will store the real values (user has not to initialize the size of this matrix).
		  * \param out_imag The N_1xN_2 output matrix which will store the imaginary values (user has not to initialize the size of this matrix).
		  * \sa fft_real, ifft2_read, fft2_complex
		  *  If the dimensions of the matrix are powers of two, the fast fourier transform (FFT) is used instead of the general algorithm.
		  */
		void  dft2_real(
			const CMatrixTemplateNumeric<float> &in_data,
			CMatrixTemplateNumeric<float>		&out_real,
			CMatrixTemplateNumeric<float>		&out_imag );

		/** Compute the 2D inverse Discrete Fourier Transform (DFT)
		  * \param in_real The N_1xN_2 input matrix with real values.
		  * \param in_imag The N_1xN_2 input matrix with imaginary values.
		  * \param out_data The N_1xN_2 output matrix (user has not to initialize the size of this matrix).
		  *  Note that the real and imaginary parts of the FFT will NOT be checked to assure that they represent the transformation
		  *    of purely real data.
		  *  If the dimensions of the matrix are powers of two, the fast fourier transform (FFT) is used instead of the general algorithm.
		  * \sa fft_real, fft2_real
		  */
		void  idft2_real(
			const CMatrixTemplateNumeric<float>	&in_real,
			const CMatrixTemplateNumeric<float>	&in_imag,
			CMatrixTemplateNumeric<float>		&out_data );

		/** Compute the 2D Discrete Fourier Transform (DFT) of a complex matrix, returning the real and imaginary parts separately.
		  * \param in_real The N_1xN_2 matrix with the real part.
		  * \param in_imag The N_1xN_2 matrix with the imaginary part.
		  * \param out_real The N_1xN_2 output matrix which will store the real values (user has not to initialize the size of this matrix).
		  * \param out_imag The N_1xN_2 output matrix which will store the imaginary values (user has not to initialize the size of this matrix).
		  *  If the dimensions of the matrix are powers of two, the fast fourier transform (FFT) is used instead of the general algorithm.
		  * \sa fft_real, idft2_complex,dft2_real
		  */
		void  dft2_complex(
			const CMatrixTemplateNumeric<float> &in_real,
			const CMatrixTemplateNumeric<float> &in_imag,
			CMatrixTemplateNumeric<float>		&out_real,
			CMatrixTemplateNumeric<float>		&out_imag);

		/** Compute the 2D inverse Discrete Fourier Transform (DFT).
		  * \param in_real The N_1xN_2 input matrix with real values, where both dimensions MUST BE powers of 2.
		  * \param in_imag The N_1xN_2 input matrix with imaginary values, where both dimensions MUST BE powers of 2.
		  * \param out_real The N_1xN_2 output matrix for real part (user has not to initialize the size of this matrix).
		  * \param out_imag The N_1xN_2 output matrix for imaginary part (user has not to initialize the size of this matrix).
		  * \sa fft_real, dft2_real,dft2_complex
		  *  If the dimensions of the matrix are powers of two, the fast fourier transform (FFT) is used instead of the general algorithm.
		  */
		void  idft2_complex(
			const CMatrixTemplateNumeric<float>	&in_real,
			const CMatrixTemplateNumeric<float>	&in_imag,
			CMatrixTemplateNumeric<float>		&out_real,
			CMatrixTemplateNumeric<float>		&out_imag );

		/** Compute the two eigenvalues of a 2x2 matrix.
		  * \param in_matrx The 2x2 input matrix.
		  * \param min_eigenvalue (out) The minimum eigenvalue of the matrix.
		  * \param max_eigenvalue (out) The maximum eigenvalue of the matrix.
		  * by FAMD, MAR-2007
		  */
		void  computeEigenValues2x2(
			const CMatrixTemplateNumeric<float>	&in_matrix,
			float								&min_eigenvalue,
			float								&max_eigenvalue );


		/** Computes the 'exp' of all the elements of a vector
		  * \sa MATH::Log
		  */
		template<class T>
			std::vector<T> Exp(const std::vector<T> &v)
		{
			std::vector<T>	ret(v.size());
			typename std::vector<T>::const_iterator it;
			typename std::vector<T>::iterator		it2;
			for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::exp(*it);
			return ret;
		}

		/** Computes the 'log' of all the elements of a vector
		  * \sa MATH::Exp
		  */
		template<class T>
			std::vector<T> Log(const std::vector<T> &v)
		{
			std::vector<T>	ret(v.size());
			typename std::vector<T>::const_iterator it;
			typename std::vector<T>::iterator		it2;
			for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::log(*it);
			return ret;
		}

	} // End of class def.

} // End of namespace


#endif
