using System.Collections.Generic;
using MikuMikuDance.XNA.Model;
using Microsoft.Xna.Framework;
using MikuMikuDance.XNA.Model.ModelData;
using System;

namespace MikuMikuDance.XNA.Motion
{

    /// <summary>
    /// 回転制限クラス
    /// </summary>
    internal class RotationLimit
    {
        /// <summary>
        /// 最大回転
        /// </summary>
        /// <remarks>X回転、Y回転、Z回転制限</remarks>
        public float[] MaxRot { get; protected set; }
        /// <summary>
        /// 最小回転
        /// </summary>
        /// <remarks>X回転、Y回転、Z回転制限</remarks>
        public float[] MinRot { get; protected set; }
        /// <summary>
        /// 角度の反射調整機能使用フラグ
        /// </summary>
        /// <remarks>IKのCCDソルブは足のIKが＜が＞になる感じの解を出してくるので、反射してやると上手くいくっぽい</remarks>
        public bool[] Mirror { get; private set; }
        /// <summary>
        /// 角度の反射調整の反発係数
        /// </summary>
        public float[] Restitution { get; private set; }
        /// <summary>
        /// 角速度の"粘性"係数。IKのソルブの過程で解が"飛ぶ"のを防ぐために設定
        /// </summary>
        public float[] Stickness { get; private set; }

        
        /// <summary>
        /// 既定のコンストラクタ
        /// </summary>
        public RotationLimit()
        {
            MaxRot = new float[3];
            MinRot = new float[3];
            Mirror = new bool[3];
            Restitution = new float[3];
            for (int i = 0; i < 3; i++)
            {
                MaxRot[i] = MathHelper.Pi;
                MinRot[i] = -MathHelper.Pi;
                Mirror[i] = false;
                Restitution[i] = 0.5f;
            }
            
            
        }
        

        /// <summary>
        /// 指定した角度をアジャストする
        /// </summary>
        /// <param name="value">回転角</param>
        /// <param name="index">回転軸</param>
        /// <returns>アジャスト済み角度</returns>
        public float Adjust(float value, int index)
        {
            if (MinRot[index] > MaxRot[index])
            {//角度が逆なら入れ替えておく
                MMDMath.Swap<float>(ref MinRot[index], ref MaxRot[index]);
            }
            if (MaxRot[index] < value)
            {
                if (Mirror[index])
                    return MaxRot[index] * (1 + Restitution[index]) - value * Restitution[index];
                else
                    return MaxRot[index];
            }
            else if (MinRot[index] > value)
            {
                if (Mirror[index])
                    return MinRot[index] * (1 + Restitution[index]) - value * Restitution[index];
                else
                    return MinRot[index];
            }
            else
                return value;
            
        }
    }
    /// <summary>
    /// IKの稼働制限を指定するクラス
    /// </summary>
    internal class IKLimitation
    {

        /// <summary>
        /// 総合稼働軸制限一覧
        /// </summary>
        /// <remarks>ボーン名マッチング用の正規表現オブジェクトと許可回転軸(親ボーン基準)</remarks>
        public Dictionary<string, RotationLimit> TotalRotationLimits { get; protected set; }



        /// <summary>
        /// 既定のコンストラクタ
        /// </summary>
        public IKLimitation()
        {
            //総合稼働制限
            TotalRotationLimits = new Dictionary<string, RotationLimit>();
            RotationLimit limit;
            limit = new RotationLimit();
            limit.MaxRot[0] = MathHelper.Pi;
            limit.MinRot[0] = MathHelper.ToRadians(3f);//3度ぐらい制限を設けてやると上手くいく。
            limit.MinRot[1] = 0;
            limit.MaxRot[1] = 0;
            limit.MinRot[2] = 0;
            limit.MaxRot[2] = 0;
            limit.Mirror[0] = true;
            limit.Restitution[0] = 0.99f;
            TotalRotationLimits.Add("左ひざ", limit);
            limit = new RotationLimit();
            limit.MaxRot[0] = MathHelper.Pi;
            limit.MinRot[0] = MathHelper.ToRadians(3f);//3度ぐらい制限を設けてやると上手くいく。
            limit.MinRot[1] = 0;
            limit.MaxRot[1] = 0;
            limit.MinRot[2] = 0;
            limit.MaxRot[2] = 0;
            limit.Mirror[0] = true;
            limit.Restitution[0] = 0.99f;
            TotalRotationLimits.Add("右ひざ", limit);
            
            //IKのソルブ及びそれの調整計算に関するメモ
            //上記数値調整計算及び各種数値設定はMMDの元コード推定(リバースエンジニアリング、逆コンパイラとかはしてないからRエンジニアって言うのか分からないけど)する過程で落ち着いている今のところの解です。
            //ほんとの解法は樋口さんが知ってるんだろうけどｗ
            //解法は今のところIK-CCD法がMMDにとって最適だと考えてます。
            //理由として
            //・ひざのボーンにIKソルブ時の角度制限が入っているっぽいので、ソルブにボーンの角度を扱う必要があること
            //・高速解法が必要であること(MMDが非常に軽いことと、イテレーションの存在とその回数を考えると、軽いアルゴリズムを使ってないとつじつまが合わない)
            //が上げられます
            //そこで、CCD,Particleかの二つで、角度を使い易かったCCDを選びました。
            //ひざの角度調整はCCDのクセを抑える理由もあって工夫してあります。
            //CCDのクセとして、正しい解が＜だとしたら、＞という解を出してくることが多いという問題があります。(＞＜は足ですｗ)
            //そのために"反発係数"なる謎なパラメータを付けてますｗ
            //また、解がほとんどまっすぐな解を出す際に、|な感じの解で固定されてしまう問題があるため、3度ぐらい下限を入れています(どうも、MMDの方も入れてるっぽいけど、よく分からない……)
            //これは現在の推定結果です。もっと再現性が高い解があれば、改造して、ぜひ教えてください
        }

        bool CheckNaN(Vector3 input)
        {
            if (float.IsNaN(input.X))
                return true;
            if (float.IsNaN(input.Y))
                return true;
            if (float.IsNaN(input.Z))
                return true;
            return false;
        }
        /// <summary>
        /// ボーン全体の動きに可動制限をかけてボーンの姿勢を修正
        /// </summary>
        /// <param name="boneIndex">ボーン番号</param>
        /// <param name="manager">ボーンマネージャ</param>
        /// <returns>修正済みボーントランスフォーム</returns>
        public virtual QuatTransform AdjustTotalBoneMove(int boneIndex, MMDBoneManager manager)
        {
            bool flag = false;
            foreach (var i in TotalRotationLimits)
            {
                if (i.Key == manager.Bones[boneIndex].BoneData.Name)
                    flag = true;
            }
            if (!flag)
                return manager.Bones[boneIndex].BoneTransform;
            //まずは軸回転のマトリクスを生成する
            Matrix bonetrans = manager.Bones[boneIndex].BoneTransform.CreateMatrix();
            Matrix bindpose = manager.Bones[boneIndex].BoneData.BindPose.CreateMatrix();
            Matrix moveMat = bonetrans * Matrix.Invert(bindpose);

            //回転を取得
            Vector3 temp, trans;
            Quaternion rot;

            moveMat.Decompose(out temp, out rot, out trans);


            float YRot, XRot, ZRot;
            int FactoringType = 0;
            //if (MMDMath.FactoringQuaternionZXY(rot, out ZRot, out XRot, out YRot))
            //まずはXYZで分解
            if (!MMDMath.FactoringQuaternionXYZ(rot, out XRot, out YRot, out ZRot))
            {//ジンバルロック対策
                //YZXで分解
                if (!MMDMath.FactoringQuaternionYZX(rot, out YRot, out ZRot, out XRot))
                {
                    //ZXYで分解
                    MMDMath.FactoringQuaternionZXY(rot, out ZRot, out XRot, out YRot);
                    FactoringType = 2;
                }
                else
                    FactoringType = 1;
            }
            else
                FactoringType = 0;

#if DEBUG
            if (float.IsNaN(XRot))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(YRot))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(ZRot))
                throw new ApplicationException("NaNを発見");
#endif
            RotationLimit lim = TotalRotationLimits[manager.Bones[boneIndex].BoneData.Name];
            XRot = lim.Adjust(XRot, 0);
            YRot = lim.Adjust(YRot, 1);
            ZRot = lim.Adjust(ZRot, 2);

            Quaternion newRot;
            if (FactoringType == 0)
                newRot = Quaternion.CreateFromRotationMatrix(
                        Matrix.CreateRotationX(XRot) *
                        Matrix.CreateRotationY(YRot) *
                        Matrix.CreateRotationZ(ZRot));
            else if (FactoringType == 1)
                newRot = Quaternion.CreateFromRotationMatrix(
                        Matrix.CreateRotationY(YRot) *
                        Matrix.CreateRotationZ(ZRot) *
                        Matrix.CreateRotationX(XRot));
            else
                newRot = Quaternion.CreateFromYawPitchRoll(YRot, XRot, ZRot);
#if DEBUG
            IK_CCDSolver.CheckNaN2(newRot);
#endif
            return new QuatTransform(newRot, trans) * manager.Bones[boneIndex].BoneData.BindPose;

        }
    }
}
