#include "utils/OsgUtils.h"

#include <osg/Math>

#include "common/SpdLogger.h"


osg::Quat OsgUtils::HPRToQuat(const osg::Vec3& angle) {
    osg::Quat q(
        osg::inDegrees(angle.y()), osg::Vec3d(0.0, 1.0, 0.0),
        osg::inDegrees(angle.x()), osg::Vec3d(1.0, 0.0, 0.0),
        osg::inDegrees(angle.z()), osg::Vec3d(0.0, 0.0, 1.0));
    return q;
}

void OsgUtils::QuatToHPR(const osg::Quat& q, osg::Vec3* angle) {
    double test = q.y() * q.z() + q.x() * q.w();
    double heading = 0, pitch = 0, roll = 0;
    if (test > 0.4999) { // singularity at north pole
        heading = 2.0 * atan2(q.y(), q.w());
        pitch = osg::PI_2;
        roll = 0.0;
        angle->set(90.0f, roll, osg::RadiansToDegrees(heading));
        return;
    }
    if (test < -0.4999) { // singularity at south pole
        heading = 2.0 * atan2(q.y(), q.w());
        pitch = -osg::PI_2;
        roll = 0.0;
        angle->set(-90.0f, roll, osg::RadiansToDegrees(heading));
        return;
    }
    double sqx = q.x() * q.x();
    double sqy = q.y() * q.y();
    double sqz = q.z() * q.z();
    heading = atan2(2.0 * q.z() * q.w() - 2.0 * q.y() * q.x(), 1.0 - 2.0 * sqz - 2.0 * sqx);
    pitch = asin(2.0 * test);
    roll = atan2(2.0 * q.y() * q.w() - 2.0 * q.z() * q.x(), 1.0 - 2.0 * sqy - 2.0 * sqx);
    angle->set(osg::RadiansToDegrees(pitch), osg::RadiansToDegrees(roll), osg::RadiansToDegrees(heading));
}

void OsgUtils::Vec4ToQColor(const osg::Vec4& vec, QColor* color) {
    int r = static_cast<int>(vec.r() * 255.0f);
    int g = static_cast<int>(vec.g() * 255.0f);
    int b = static_cast<int>(vec.b() * 255.0f);
    int a = static_cast<int>(vec.a() * 255.0f);
    color->setRgb(r, g, b, a);
}

void OsgUtils::QColorToVec4(const QColor& color, osg::Vec4* vec) {
    float r = color.red() / 255.0f;
    float g = color.green() / 255.0f;
    float b = color.blue() / 255.0f;
    float a = color.alpha() / 255.0f;
    vec->set(r, g, b, a);
}