using System;
using Microsoft.Xna.Framework;
using BulletX.LinerMath;

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="ZRot">Z軸回転</param>
        /// <param name="XRot">X軸回転(-PI/2～PI/2)</param>
        /// <param name="YRot">Y軸回転</param>
        /// <returns>ジンバルロックが発生した時はfalse。ジンバルロックはX軸回転で発生</returns>
        public static bool FactoringQuaternionZXY(Quaternion input, out float ZRot, out float XRot, out float YRot)
        {
            //クォータニオンの正規化
            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.M13, rot.M11);
                return false;
            }
            XRot = -(float)Math.Asin(rot.M32);
            //ロールを取得
            ZRot = (float)Math.Asin(rot.M12 / Math.Cos(XRot));
            if (float.IsNaN(ZRot))
            {//漏れ対策
                XRot = (rot.M32 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
                ZRot = 0; YRot = (float)Math.Atan2(-rot.M13, rot.M11);
                return false;
            }
            if (rot.M22 < 0)
                ZRot = MathHelper.Pi - ZRot;
            //ピッチを取得
            YRot = (float)Math.Atan2(rot.M31, rot.M33);
            return true;
        }
        /// <summary>
        /// クォータニオンをX,Y,Z回転に分解する関数
        /// </summary>
        /// <param name="input">分解するクォータニオン</param>
        /// <param name="XRot">X軸回転</param>
        /// <param name="YRot">Y軸回転(-PI/2～PI/2)</param>
        /// <param name="ZRot">Z軸回転</param>
        /// <returns></returns>
        public static bool FactoringQuaternionXYZ(Quaternion input, out float XRot, out float YRot, out float ZRot)
        {
            //クォータニオンの正規化
            Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
            inputQ.Normalize();
            //マトリクスを生成する
            Matrix rot = Matrix.CreateFromQuaternion(inputQ);
            //Y軸回りの回転を取得
            if (rot.M13 > 1 - 1.0e-4 || rot.M13 < -1 + 1.0e-4)
            {//ジンバルロック判定
                XRot = 0;
                YRot = (rot.M13 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
                ZRot = -(float)Math.Atan2(-rot.M21, rot.M22);
                return false;
            }
            YRot = -(float)Math.Asin(rot.M13);
            //X軸回りの回転を取得
            XRot = (float)Math.Asin(rot.M23 / Math.Cos(YRot));
            if (float.IsNaN(XRot))
            {//ジンバルロック判定(漏れ対策)
                XRot = 0;
                YRot = (rot.M13 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
                ZRot = -(float)Math.Atan2(-rot.M21, rot.M22);
                return false;
            }
            if (rot.M33 < 0)
                XRot = MathHelper.Pi - XRot;
            //Z軸回りの回転を取得
            ZRot = (float)Math.Atan2(rot.M12, rot.M11);
            return true;
        }
        /// <summary>
        /// クォータニオンをY,Z,X回転に分解する関数
        /// </summary>
        /// <param name="input">分解するクォータニオン</param>
        /// <param name="YRot">Y軸回転</param>
        /// <param name="ZRot">Z軸回転(-PI/2～PI/2)</param>
        /// <param name="XRot">X軸回転</param>
        /// <returns></returns>
        public static bool FactoringQuaternionYZX(Quaternion input, out float YRot, out float ZRot, out float XRot)
        {
            //クォータニオンの正規化
            Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
            inputQ.Normalize();
            //マトリクスを生成する
            Matrix rot = Matrix.CreateFromQuaternion(inputQ);
            //Z軸回りの回転を取得
            if (rot.M21 > 1 - 1.0e-4 || rot.M21 < -1 + 1.0e-4)
            {//ジンバルロック判定
                YRot = 0;
                ZRot = (rot.M21 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
                XRot = -(float)Math.Atan2(-rot.M32, rot.M33);
                return false;
            }
            ZRot = -(float)Math.Asin(rot.M21);
            //Y軸回りの回転を取得
            YRot = (float)Math.Asin(rot.M31 / Math.Cos(ZRot));
            if (float.IsNaN(YRot))
            {//ジンバルロック判定(漏れ対策)
                YRot = 0;
                ZRot = (rot.M21 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
                XRot = -(float)Math.Atan2(-rot.M32, rot.M33);
                return false;
            }
            if (rot.M11 < 0)
                YRot = MathHelper.Pi - YRot;
            //X軸回りの回転を取得
            XRot = (float)Math.Atan2(rot.M23, 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);
        }


        
    }
}
