/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#pragma once

#define _USE_MATH_DEFINES // (For VS to define M_PI, etc. in cmath)
#include <cmath> // floor()
#include <algorithm> // reverse()

/** This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries. */
namespace mrpt
{
	/** A std::string version of C sprintf.
	  *  You can call this to obtain a std::string using printf-like syntax.
	  *  Based on very nice code by Paul Senzee, published at http://senzee.blogspot.com/2006/05/c-formatting-stdstring.html
	  *  Function implemented in format.cpp
	  */
	std::string BASE_IMPEXP format(const char *fmt, ...) MRPT_printf_format_check(1,2);

	namespace math
	{
		bool BASE_IMPEXP isNaN(float v) MRPT_NO_THROWS;
		bool BASE_IMPEXP isNaN(double v) MRPT_NO_THROWS;
		bool BASE_IMPEXP isFinite(float v) MRPT_NO_THROWS;
		bool BASE_IMPEXP isFinite(double v) MRPT_NO_THROWS;

		// This inline function is used everywhere, so just move it here even it's not a forward declaration!
		/*! Returns the size of the matrix in the i'th dimension: 1=rows, 2=columns (MATLAB-compatible function)
		  *  \note Template argument MATRIXLIKE can be: mrpt::math::CMatrixTemplate, mrpt::math::CMatrixTemplateNumeric, mrpt::math::CMatrixFixedNumeric
		  */
		template <class MATRIXLIKE>
		inline size_t size( const MATRIXLIKE& m, int dim )
		{
			if (dim==1) return m.getRowCount();
			else if (dim==2) return m.getColCount();
			else THROW_EXCEPTION_CUSTOM_MSG1("size: Queried matrix dimension must be 1 or 2. Called with i=%i",dim);
		}
	}

	namespace utils
	{
		class CFileStream;
		void BASE_IMPEXP global_profiler_enter(const char *func_name) MRPT_NO_THROWS;
		void BASE_IMPEXP global_profiler_leave(const char *func_name) MRPT_NO_THROWS;

		struct CProfilerProxy {
			const char*f;
			CProfilerProxy(const char*func_name) : f(func_name) { global_profiler_enter(f); }
			~CProfilerProxy() { global_profiler_leave(f); }
		};

#ifdef DEG2RAD  // functions are preferred over macros
#undef DEG2RAD
#endif
#ifdef RAD2DEG
#undef RAD2DEG
#endif
#if !defined(M_PI)
#	define M_PI 3.14159265358979323846
#endif

		/** Degrees to radians */
		inline double DEG2RAD(const double x) { return x*M_PI/180.0;	}
		/** Degrees to radians */
		inline float DEG2RAD(const float x) { return x*M_PIf/180.0f; }
		/** Degrees to radians */
		inline float DEG2RAD(const int x) { return x*M_PIf/180.0f; }
		/** Radians to degrees */
		inline double RAD2DEG(const double x) { return x*180.0/M_PI; }
		/** Radians to degrees */
		inline float RAD2DEG(const float x) { return x*180.0f/M_PIf; }

#	ifdef HAVE_LONG_DOUBLE
		/** Degrees to radians */
		inline long double DEG2RAD(const long double x) { return x*M_PIl/180.0;	}
		/** Radians to degrees */
		inline long double RAD2DEG(const long double x) { return x*180.0/M_PIl; }
#	endif

#define DEG2RAD DEG2RAD  // This is required to avoid other libs (like PCL) to #define their own versions of DEG2RAD
#define RAD2DEG RAD2DEG  // This is required to avoid other libs (like PCL) to #define their own versions of RAD2DEG

		/** Returns the sign of X as "1" or "-1" */
		template <typename T>
		inline int sign(T x) { return x<0 ? -1:1; }

		/** Returns the sign of X as "0", "1" or "-1" */
		template <typename T>
		inline int signWithZero(T x)	{ return x==0?0:sign(x);}

		/** Efficient and portable evaluation of the absolute difference of two unsigned integer values
		  * (but will also work for signed and floating point types) */
		template <typename T>
		inline T abs_diff(const T a, const T b) {
			return std::max(a,b) - std::min(a,b);
		}

		template<typename T> inline const T  min3(const T& A, const T& B,const T& C) { return std::min<T>(A, std::min<T>(B,C) ); }
		template<typename T> inline const T  max3(const T& A, const T& B,const T& C) { return std::max<T>(A, std::max<T>(B,C) ); }

		/** Rounds toward zero  */
		template <typename T>
		inline int fix(T x) { return  x>0 ? static_cast<int>(floor(static_cast<double>(x))) : static_cast<int>(ceil(static_cast<double>(x))) ; }

		/** Inline function for the square of a number. */
		template<class T>
		inline T square(const T x)    { return x*x; }

		/** Utility to get a cast'ed pointer from a smart pointer */
		template <class R, class SMART_PTR>
		inline R* getAs(SMART_PTR &o) { return static_cast<R*>( & (*o) ); }

		/** Utility to get a cast'ed pointer from a smart pointer */
		template <class R, class SMART_PTR>
		inline const R* getAs(const SMART_PTR &o) { return static_cast<const R*>( & (*o) ); }

		/** Reverse the order of the bytes of a given type (useful for transforming btw little/big endian)  */
		template <class T> inline void reverseBytesInPlace(T& v_in_out)
		{
			unsigned char *ptr = reinterpret_cast<unsigned char*>(&v_in_out);
			std::reverse(ptr,ptr+sizeof(T));
		}

		/** Reverse the order of the bytes of a given type (useful for transforming btw little/big endian)  */
		template <class T> inline void reverseBytes(const T &v_in, T& v_out)
		{
			v_out = v_in;
			reverseBytesInPlace(v_out);
		}


		/** If the second argument is below the first one, set the first argument to this lower value. */
		template <typename T,typename K>
		inline void keep_min(T &var,  const K test_val) {
			if (test_val<var) var = test_val;
		}

		/** If the second argument is above the first one, set the first argument to this higher value. */
		template <typename T,typename K>
		inline void keep_max(T &var,  const K test_val) {
			if (test_val>var) var = test_val;
		}

		/** Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL. */
		template <class T>
		void delete_safe(T *& ptr) {
			if (ptr) {
				delete ptr;
				ptr = NULL;
			}
		}

		/** Like calling a std::vector<>'s clear() method, but really forcing deallocating the memory. */
		template <class VECTOR_T>
		inline void vector_strong_clear(VECTOR_T & v) { VECTOR_T dummy; dummy.swap(v); }

	} // End of namespace
} // end of namespace
