#include "shared/math/angles.hh"

namespace shared {
namespace math {

float angles::get_yaw_delta(const float& a, const float& b) noexcept {
    const float inner = glm::distance(a, b);
    const float outer = glm::radians(360.0f) - inner;

    const bool less = a < b;
    return inner <= outer ? (less ? inner : -inner) : (less ? -outer : outer);
}

glm::vec3 angles::to_dir() const noexcept {
    const float x = std::cos(this->pitch) * std::cos(this->yaw);
    const float y = std::sin(this->pitch);
    const float z = std::cos(this->pitch) * std::sin(this->yaw);
    return {x, y, z};
}

angles& angles::clamp() noexcept {
    this->pitch =
        std::clamp(this->pitch, glm::radians(-89.0f), glm::radians(89.0f));
    this->yaw =
        std::clamp(this->yaw, glm::radians(-180.0f), glm::radians(180.0f));
    return *this;
}

angles& angles::normalise() noexcept {
    this->yaw = std::fmod(this->yaw, glm::radians(360.0f));

    if (const auto fmod = std::fmod(this->yaw, glm::radians(180.0f));
        fmod != this->yaw) {

        this->yaw = fmod + (std::signbit(this->yaw) ? glm::radians(180.0f)
                                                    : glm::radians(-180.0f));
    }

    return *this;
}

angles angles::operator+(const angles& a) const noexcept {
    auto ret = *this;
    ret.pitch += a.pitch;
    ret.yaw += a.yaw;
    return ret;
}

angles angles::operator*(const float& f) const noexcept {
    auto ret = *this;
    ret.pitch *= f;
    ret.yaw *= f;
    return ret;
}

} // namespace math
} // namespace shared