/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/abs_diff.h>  // mrpt::abs_diff()

#include <algorithm>  // max()
#include <cmath>      // floor(),isnan(),...
#include <cstdint>
#include <stdexcept>

namespace mrpt
{
/** \addtogroup mrpt_bits_math Funtions in #include <mrpt/core/bits_math.h>
 *  \ingroup mrpt_core_grp
 * @{ */

/** Inline function for the square of a number. */
template <typename num_t, typename return_t = num_t>
inline return_t square(const num_t x)
{
  return static_cast<return_t>(x * x);
}

/** Faster version of std::hypot(), to use when overflow is not an issue and we
 * prefer fast code. */
template <class T>
inline T hypot_fast(const T x, const T y)
{
  return std::sqrt(x * x + y * y);
}

#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  */
constexpr inline double DEG2RAD(const double x) { return x * M_PI / 180.0; }
/** Degrees to radians */
constexpr inline float DEG2RAD(const float x) { return x * float(M_PI) / 180.0f; }
/** Degrees to radians */
constexpr inline double DEG2RAD(const int x) { return x * M_PI / 180.0; }
/** Radians to degrees */
constexpr inline double RAD2DEG(const double x) { return x * 180.0 / M_PI; }
/** Radians to degrees */
constexpr inline float RAD2DEG(const float x) { return x * 180.0f / float(M_PI); }
#if !defined(M_PIl)
#define M_PIl  3.14159265358979323846264338327950288L
#define M_2PIl (2.0L * 3.14159265358979323846264338327950288L)
#endif
/** Degrees to radians */
constexpr inline long double DEG2RAD(const long double x) { return x * M_PIl / 180.0; }
/** Radians to degrees */
constexpr inline long double RAD2DEG(const long double x) { return x * 180.0 / M_PIl; }

// This is required to avoid other libs (like PCL) to #define their own macros
// after including this header
#define DEG2RAD DEG2RAD
#define RAD2DEG RAD2DEG

namespace literals
{
/** degrees to radian literal operator (e.g. `x=90.0_deg;`) */
constexpr inline double operator"" _deg(long double v)
{
  return static_cast<double>(mrpt::DEG2RAD(v));
}
}  // namespace literals
using namespace mrpt::literals;  // for backwards compatib.

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

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

/** Returns the smallest positive number among a and b */
template <typename T>
[[nodiscard]] T lowestPositive(const T a, const T b)
{
  if (a > 0 && a <= b)
    return a;  // a positive and smaller than b
  else if (b > 0)
    return b;  // b is positive and either smaller than a or a is negative
  else
    return a;  // at least b is negative, we might not have an answer
}

template <typename T>
[[nodiscard]] 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>
[[nodiscard]] 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>
[[nodiscard]] inline int fix(T x)
{
  return x > 0 ? static_cast<int>(floor(static_cast<double>(x)))
               : static_cast<int>(ceil(static_cast<double>(x)));
}

/** 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 = static_cast<T>(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 = static_cast<T>(test_val);
}
/** Saturate the value of var (the variable gets modified) so it does not get
 * out of [min,max]. */
template <typename T>
inline void saturate(T& var, const T sat_min, const T sat_max)
{
  if (var > sat_max) var = sat_max;
  if (var < sat_min) var = sat_min;
}
/** Like saturate() but it returns the value instead of modifying the variable
 */
template <typename T>
[[nodiscard]] inline T saturate_val(const T& value, const T sat_min, const T sat_max)
{
  T var = value;
  if (var > sat_max) var = sat_max;
  if (var < sat_min) var = sat_min;
  return var;
}

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

/** shortcut for static_cast<float>(double) */
[[nodiscard]] inline float d2f(const double d) { return static_cast<float>(d); }

/** converts a float [0,1] into an uint8_t [0,255] (without checking for out of
 * bounds) \sa u8tof */
[[nodiscard]] inline uint8_t f2u8(const float f) { return static_cast<uint8_t>(f * 255); }

/** converts a uint8_t [0,255] into a float [0,1] \sa f2u8 */
[[nodiscard]] inline float u8tof(const uint8_t v) { return v / 255.0f; }

/** @} */

}  // namespace mrpt
