using UnityEngine;

namespace URDF
{
    /// <summary>
    /// Utility class for converting between URDF coordinate system (right-handed, Z-up)
    /// and Unity coordinate system (left-handed, Y-up)
    /// </summary>
    public static class URDFCoordinateConverter
    {
        /// <summary>
        /// Convert URDF position (right-handed, Z-up) to Unity position (left-handed, Y-up)
        /// Conversion: (x, y, z) -> (x, -z, y)
        /// </summary>
        public static Vector3 ConvertPosition(Vector3 urdfPosition)
        {
            return new Vector3(urdfPosition.x, -urdfPosition.z, urdfPosition.y);
        }

        /// <summary>
        /// Convert URDF rotation (RPY in right-handed, Z-up) to Unity rotation (left-handed, Y-up)
        /// </summary>
        public static Quaternion ConvertRotation(Vector3 rpy)
        {
            // Convert RPY to Euler angles in Unity coordinate system
            // RPY: Roll (around X), Pitch (around Y), Yaw (around Z)
            // Unity: X, Y, Z rotations in left-handed system
            
            // First convert RPY to quaternion in URDF system, then convert to Unity
            Quaternion roll = Quaternion.AngleAxis(rpy.x * Mathf.Rad2Deg, Vector3.right);
            Quaternion pitch = Quaternion.AngleAxis(rpy.y * Mathf.Rad2Deg, Vector3.up);
            Quaternion yaw = Quaternion.AngleAxis(rpy.z * Mathf.Rad2Deg, Vector3.forward);
            
            // In URDF: rotation order is typically Roll-Pitch-Yaw
            Quaternion urdfRotation = yaw * pitch * roll;
            
            // Convert from URDF (right-handed, Z-up) to Unity (left-handed, Y-up)
            // This is a coordinate system transformation
            Matrix4x4 urdfToUnity = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, Vector3.one);
            urdfToUnity.m00 = 1; urdfToUnity.m01 = 0; urdfToUnity.m02 = 0;
            urdfToUnity.m10 = 0; urdfToUnity.m11 = 0; urdfToUnity.m12 = -1;
            urdfToUnity.m20 = 0; urdfToUnity.m21 = 1; urdfToUnity.m22 = 0;
            
            // Alternative approach: directly convert Euler angles
            // URDF: X=right, Y=forward, Z=up
            // Unity: X=right, Y=up, Z=forward
            // So: Unity X = URDF X, Unity Y = URDF Z, Unity Z = -URDF Y
            
            float unityX = rpy.x * Mathf.Rad2Deg; // Roll stays the same
            float unityY = rpy.z * Mathf.Rad2Deg; // Yaw becomes Y rotation
            float unityZ = -rpy.y * Mathf.Rad2Deg; // Pitch becomes -Z rotation
            
            return Quaternion.Euler(unityX, unityY, unityZ);
        }

        /// <summary>
        /// Convert URDF axis (right-handed, Z-up) to Unity axis (left-handed, Y-up)
        /// </summary>
        public static Vector3 ConvertAxis(Vector3 urdfAxis)
        {
            return new Vector3(urdfAxis.x, -urdfAxis.z, urdfAxis.y);
        }
    }
}