#ifndef _MANIF_MANIF_CONSTANTS_H_
#define _MANIF_MANIF_CONSTANTS_H_

#include <cmath>
#include <limits>

#define MANIF_PI   3.141592653589793238462643383279502884
#define MANIF_PI_2 1.570796326794896619231321691639751442
#define MANIF_PI_4 0.785398163397448309615660845819875721

namespace manif {
namespace internal {

/**
 * Constexpr Newton-Raphson iterative algorithm for the sqrt aprrox.
 */
template <typename T>
T constexpr sqrtNewtonRaphson(T x, T curr, T prev)
{
  return curr == prev
          ? curr
          : sqrtNewtonRaphson(x, T(0.5) * (curr + x / curr), curr);
}

/**
 * Constexpr version of the square root
 * Return value:
 *   - For a finite and non-negative value of "x",
 *     returns an approximation for the square root of "x"
 *   - Otherwise, returns NaN
 *
 * credits : https://stackoverflow.com/a/34134071/9709397
 */
template <typename T>
T constexpr csqrt(T x)
{
  return x >= T(0) && x < std::numeric_limits<T>::infinity()
        ? sqrtNewtonRaphson(x, x, T(0))
        : std::numeric_limits<T>::quiet_NaN();
}

} /* namespace internal */

/**
 * @brief Traits to define some constant scalar.
 */
template <typename _Scalar>
struct Constants
{
  static constexpr _Scalar eps      = std::numeric_limits<_Scalar>::epsilon()*_Scalar(100);
  static constexpr _Scalar eps_sqrt = internal::csqrt(eps);

  static constexpr _Scalar to_rad = _Scalar(MANIF_PI / 180.0);
  static constexpr _Scalar to_deg = _Scalar(180.0 / MANIF_PI);
};

template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::eps;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::eps_sqrt;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::to_rad;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::to_deg;

} /* namespace manif  */

#endif /* _MANIF_MANIF_CONSTANTS_H_ */