/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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 <cmath>
#include <cstddef>	// size_t

namespace mrpt::math
{
/** \addtogroup container_ops_grp
 * @{ */

/** Modifies the given angle to translate it into the [0,2pi[ range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
 */
template <class T>
inline void wrapTo2PiInPlace(T& a)
{
	bool was_neg = a < 0;
	a = static_cast<T>(fmod(a, static_cast<T>(2.0 * M_PI)));
	if (was_neg) a += static_cast<T>(2.0 * M_PI);
}

/** Modifies the given angle to translate it into the [0,2pi[ range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
 */
template <class T>
inline T wrapTo2Pi(T a)
{
	wrapTo2PiInPlace(a);
	return a;
}

/** Modifies the given angle to translate it into the ]-pi,pi] range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
 */
template <class T>
inline T wrapToPi(T a)
{
	return wrapTo2Pi(a + static_cast<T>(M_PI)) - static_cast<T>(M_PI);
}

/** Modifies the given angle to translate it into the ]-pi,pi] range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
 */
template <class T>
inline void wrapToPiInPlace(T& a)
{
	a = wrapToPi(a);
}

/** Modify a sequence of angle values such as no consecutive values have a jump
 * larger than PI in absolute value.
 * \sa wrapToPi
 */
template <class VECTOR>
void unwrap2PiSequence(VECTOR& x)
{
	const size_t N = x.size();
	for (size_t i = 0; i < N; i++)
	{
		mrpt::math::wrapToPiInPlace(x[i]);	// assure it's in the -pi,pi range.
		if (!i) continue;
		double Ap = x[i] - x[i - 1];
		if (Ap > M_PI) x[i] -= 2. * M_PI;
		if (Ap < -M_PI) x[i] += 2. * M_PI;
	}
}

/** Computes the shortest angular increment (or distance) between two planar
 * orientations,
 * such that it is constrained to [-pi,pi] and is correct for any combination
 * of angles (e.g. near +-pi)
 * Examples: angDistance(0,pi) -> +pi; angDistance(pi,0) -> -pi;
 *           angDistance(-3.1,3.1) -> -0.08; angDistance(3.1,-3.1) -> +0.08;
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi, wrapTo2Pi
 */
template <class T>
inline T angDistance(T from, T to)
{
	wrapToPiInPlace(from);
	wrapToPiInPlace(to);
	T d = to - from;
	if (d > M_PI) d -= 2. * M_PI;
	else if (d < -M_PI)
		d += 2. * M_PI;
	return d;
}

/** @} */

}  // namespace mrpt::math
