using System;
using Microsoft.Xna.Framework;

namespace MikuMikuDance.XNA
{
    /// <summary>
    /// 回転行列周りのヘルパクラス
    /// </summary>
    public static class MMDMath
    {
        /// <summary>
        /// 3要素配列から3次元ベクトルを生成
        /// </summary>
        /// <param name="vec">3つの要素をもった配列</param>
        /// <returns>生成済みベクトル</returns>
        public static Vector3 VectorFromArray(float[] vec)
        {
            return new Vector3(vec[0], vec[1], vec[2]);
        }

        /// <summary>
        /// MinMax関係が成り立つように各要素を修正
        /// </summary>
        /// <param name="min">最小</param>
        /// <param name="max">最大</param>
        public static void CheckMinMax(ref Vector3 min, ref Vector3 max)
        {
            if (min.X > max.X)
                Swap(ref min.X, ref max.X);
            if (min.Y > max.Y)
                Swap(ref min.Y, ref max.Y);
            if (min.Z > max.Z)
                Swap(ref min.Z, ref max.Z);
        }
        /// <summary>
        /// swap関数
        /// </summary>
        /// <typeparam name="T">スワップする型</typeparam>
        /// <param name="v1">変数1</param>
        /// <param name="v2">変数2</param>
        public static void Swap<T>(ref T v1, ref T v2)
        {
            T v3 = v1;
            v1 = v2;
            v2 = v1;
        }
        /// <summary>
        /// クォータニオンをYaw(Y回転), Pitch(X回転), Roll(Z回転)に分解する関数
        /// </summary>
        /// <param name="input">分解するクォータニオン</param>
        /// <param name="YRot">Y軸回転</param>
        /// <param name="XRot">X軸回転(-PI/2～PI/2)</param>
        /// <param name="ZRot">Z軸回転</param>
        /// <returns>ジンバルロックが発生した時はfalse。ジンバルロックはX軸回転で発生</returns>
        public static bool DecompositeQuaternion(Quaternion input, out float YRot, out float XRot, out float ZRot)
        {
            //クォータニオンの正規化
            Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
            inputQ.Normalize();
            //マトリクスを生成する
            Matrix rot = Matrix.CreateFromQuaternion(inputQ);
            //ヨー(X軸周りの回転)を取得
            if (rot.M32 > 1 - 1.0e-4 || rot.M32 < -1 + 1.0e-4)
            {//ジンバルロック判定
                XRot = (rot.M32 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
                ZRot = 0; YRot = -(float)Math.Atan2(rot.M21, rot.M11);
                
                return false;
            }
            XRot = -(float)Math.Asin(rot.M32);
            //ピッチを取得
            YRot = -(float)Math.Atan2(-rot.M31, rot.M33);
            //ロールを取得
            ZRot = -(float)Math.Atan2(-rot.M12, rot.M22);
            return true;
        }

        /// <summary>
        /// MathHelperのClampの拡張。maxとminが逆でも動作する。
        /// </summary>
        /// <param name="value">調整する値</param>
        /// <param name="min">値1</param>
        /// <param name="max">値2</param>
        /// <returns>値1と2の間に調整された値</returns>
        public static float ClampEx(float value, float min, float max)
        {
            if (min <= max)
                return MathHelper.Clamp(value, min, max);
            return MathHelper.Clamp(value, max, min);
        }
        
    }
}
