#include <cmath>
#include "geo/private/util/angle_util.h"

namespace ns
{
namespace geo
{

double normalize_angle(double theta)
{
    return std::remainder(theta, 2 * c_pi());
}

double average_angle(double theta1, double theta2)
{
    return normalize_angle(theta1 + normalize_angle(theta2 - theta1) / 2);
}

double angle_diff(double theta1, double theta2)
{
    double norm1 = normalize_angle(theta1);
    double norm2 = normalize_angle(theta2);

    double diff = std::abs(norm1 - norm2);

    if (diff > c_pi())
    {
        diff = 2 * c_pi() - diff;
    }
    return diff;
}

} // namespace geo
} // namespace ns