#ifndef EULERUTILS_HPP
#define EULERUTILS_HPP
#pragma once
#include <Ogre.h>
#include <cmath>
#include <algorithm>

namespace EulerUtils {

    // Convert Euler angles (in degrees) to quaternion
    // Order: Pitch (X), Yaw (Y), Roll (Z)
    inline Ogre::Quaternion fromEulerDegrees(const Ogre::Vector3& degrees) {
        Ogre::Radian pitch(Ogre::Degree(degrees.x));
        Ogre::Radian yaw(Ogre::Degree(degrees.y));
        Ogre::Radian roll(Ogre::Degree(degrees.z));

        // Create quaternions for each axis
        Ogre::Quaternion qPitch(pitch, Ogre::Vector3::UNIT_X);
        Ogre::Quaternion qYaw(yaw, Ogre::Vector3::UNIT_Y);
        Ogre::Quaternion qRoll(roll, Ogre::Vector3::UNIT_Z);

        // Combine in ZYX order (roll, yaw, pitch)
        return qYaw * qPitch * qRoll;
    }

    // Convert quaternion to Euler angles (in degrees)
    // Returns Vector3 (pitch, yaw, roll) in degrees
    inline Ogre::Vector3 toEulerDegrees(const Ogre::Quaternion& quat) {
        // Extract rotation matrix from quaternion
        Ogre::Matrix3 rotMat;
        quat.ToRotationMatrix(rotMat);

        // Extract Euler angles from rotation matrix
        // Using XYZ (pitch, yaw, roll) order
        Ogre::Radian pitch, yaw, roll;
        rotMat.ToEulerAnglesXYZ(pitch, yaw, roll);

        return Ogre::Vector3(
            pitch.valueDegrees(),
            yaw.valueDegrees(),
            roll.valueDegrees()
        );
    }

    // Alternative implementation using quaternion math (more stable)
    inline Ogre::Vector3 toEulerDegreesStable(const Ogre::Quaternion& quat) {
        // Extract quaternion components
        float w = quat.w;
        float x = quat.x;
        float y = quat.y;
        float z = quat.z;

        // Pitch (X-axis rotation)
        float sinr_cosp = 2.0f * (w * x + y * z);
        float cosr_cosp = 1.0f - 2.0f * (x * x + y * y);
        float pitch = std::atan2(sinr_cosp, cosr_cosp);

        // Yaw (Y-axis rotation)
        float sinp = 2.0f * (w * y - z * x);
        float yaw;
        if (std::abs(sinp) >= 1.0f) {
            yaw = std::copysign(Ogre::Math::HALF_PI, sinp);
        } else {
            yaw = std::asin(sinp);
        }

        // Roll (Z-axis rotation)
        float siny_cosp = 2.0f * (w * z + x * y);
        float cosy_cosp = 1.0f - 2.0f * (y * y + z * z);
        float roll = std::atan2(siny_cosp, cosy_cosp);

        return Ogre::Vector3(
            Ogre::Radian(pitch).valueDegrees(),
            Ogre::Radian(yaw).valueDegrees(),
            Ogre::Radian(roll).valueDegrees()
        );
    }

    // Apply rotation to a vector using quaternion
    inline Ogre::Vector3 rotateVector(const Ogre::Quaternion& quat, const Ogre::Vector3& vec) {
        return quat * vec;
    }

    // Clamp Euler angles to reasonable ranges
    inline Ogre::Vector3 clampEulerDegrees(const Ogre::Vector3& euler, float minDeg = -180.0f, float maxDeg = 180.0f) {
        return Ogre::Vector3(
            std::clamp(euler.x, minDeg, maxDeg),
            std::clamp(euler.y, minDeg, maxDeg),
            std::clamp(euler.z, minDeg, maxDeg)
        );
    }

    // Normalize Euler angles to [-180, 180] range
    inline Ogre::Vector3 normalizeEulerDegrees(const Ogre::Vector3& euler) {
        return Ogre::Vector3(
            std::fmod(euler.x + 180.0f, 360.0f) - 180.0f,
            std::fmod(euler.y + 180.0f, 360.0f) - 180.0f,
            std::fmod(euler.z + 180.0f, 360.0f) - 180.0f
        );
    }
}
#endif // EULERUTILS_HPP
