#ifndef ANGLES_ANGLES_H
#define ANGLES_ANGLES_H

#include <cmath>

namespace angles
{

/**
 * @brief Normalize an angle to the range [-π, π]
 * @param angle The angle in radians to normalize
 * @return The normalized angle in the range [-π, π]
 */
inline double normalize_angle(double angle)
{
  while (angle > M_PI)
    angle -= 2.0 * M_PI;
  while (angle < -M_PI)
    angle += 2.0 * M_PI;
  return angle;
}

/**
 * @brief Calculate the shortest angular distance between two angles
 * 
 * Computes the shortest angular distance from angle 'from' to angle 'to'.
 * The result is in the range [-π, π], where positive values indicate
 * counter-clockwise rotation and negative values indicate clockwise rotation.
 * 
 * @param from The starting angle in radians
 * @param to The target angle in radians
 * @return The shortest angular distance in radians, in the range [-π, π]
 * 
 * @code
 * double diff = angles::shortest_angular_distance(0.0, M_PI/2);  // Returns π/2
 * double diff2 = angles::shortest_angular_distance(M_PI, 0.0);  // Returns -π (or π, normalized)
 * @endcode
 */
inline double shortest_angular_distance(double from, double to)
{
  double diff = to - from;
  return normalize_angle(diff);
}

} // namespace angles

#endif // ANGLES_ANGLES_H

