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

namespace MikuMikuDance.XNA.Motion
{
    /// <summary>
    /// IKのソルバ列挙子
    /// </summary>
    public enum IKSolveType
    {
        /// <summary>
        /// CCD法
        /// </summary>
        CyclicCoordinateDescent = 1,
        /// <summary>
        /// Particle方
        /// </summary>
        Particle=2,
        /// <summary>
        /// ユーザー定義ソルバ
        /// </summary>
        User = 999,
    }
    /// <summary>
    /// IKソルバインターフェイス
    /// </summary>
    internal interface IKSolver
    {
        /// <summary>
        /// IKのソルブ
        /// </summary>
        /// <param name="Movement">IKの位置(モデル全体の座標系)</param>
        /// <param name="bones">IK影響下のリスト(IKTargetからIK影響外の親まで逆順で)</param>
        /// <param name="ikBone">IKボーン</param>
        /// <param name="manager">ボーンマネージャ</param>
        /// <param name="limitation">IK回転方向制限オブジェクト</param>
        void Solve(Vector3 Movement, List<int> bones, MMDBone ikBone, MMDBoneManager manager, IKLimitation limitation);
        /// <summary>
        /// ソルバの種別
        /// </summary>
        IKSolveType Type { get; }

    }
    /// <summary>
    /// Cyclic-Coordinate-Descent法によるIKソルバー
    /// </summary>
    internal class IK_CCDSolver : IKSolver
    {
        const double errToleranceSq = 1.0e-8f;
        /// <summary>
        /// ソルバの種別
        /// </summary>
        public IKSolveType Type { get { return IKSolveType.CyclicCoordinateDescent; } }
        /// <summary>
        /// Cyclic-Coordinate-Descent法によるソルブ
        /// </summary>
        /// <param name="Movement">IKの位置(モデル全体の座標系)</param>
        /// <param name="bones">IK影響下のリスト(IKTargetからIK影響外の親まで逆順で)</param>
        /// <param name="ikBone">IKボーン</param>
        /// <param name="manager">ボーンマネージャ</param>
        /// <param name="limitation">IK回転方向制限オブジェクト</param>
        public void Solve(Vector3 Movement, List<int> bones, MMDBone ikBone, MMDBoneManager manager, IKLimitation limitation)
        {
            //再帰回数を取得
            ushort MaxIterations = ikBone.BoneData.IK.Iteration;

            Vector3 localTargetPos = Vector3.One;
            Vector3 localEffectorPos = Vector3.Zero;
            //manager.UpdateWorldTransforms();
            //IKでソルブしない固定点を取得
            QuatTransform IKBase = manager.GetWorldQuatTransform(manager[bones.Last()].BoneData.SkeletonHierarchy);
            for (ushort it = 0; it < MaxIterations; it++)
            {
                for (int i = 1; i < bones.Count; i++)
                {//IKボーンを逆順に処理

                    //現在検索中のボーンの根元の位置を取得
                    QuatTransform qtrans = IKBase;
                    for (int j = bones.Count - 1; j >= i; j--)
                    {
                        qtrans = manager[bones[j]].BoneTransform * qtrans;
                    }
                    //エフェクタの現在位置を取得
                    //Vector3 objectVecLoc;
                    QuatTransform objectLoc = qtrans;
                    for (int j = i - 1; j >= 0; j--)
                    {
                        objectLoc = manager[bones[j]].BoneTransform * objectLoc;
                    }
                    
                    Matrix invCoord = Matrix.Invert(qtrans.CreateMatrix());
                    // 各ベクトルの座標変換を行い、検索中のボーンi基準の座標系にする
                    // (1) 基準関節i→エフェクタ位置へのベクトル(a)(ボーンi基準座標系)
                    localEffectorPos = Vector3.Transform(objectLoc.Translation, invCoord);
                    // (2) 基準関節i→目標位置へのベクトル(b)(ボーンi基準座標系)
                    localTargetPos = Vector3.Transform(Movement, invCoord);
                    
                    if ((localEffectorPos - localTargetPos).LengthSquared() < errToleranceSq)
                    {
                        return;
                    }
                    
                    // (1) 基準関節→エフェクタ位置への方向ベクトル
                    Vector3 basis2Effector = Vector3.Normalize(localEffectorPos);
                    // (2) 基準関節→目標位置への方向ベクトル
                    Vector3 basis2Target = Vector3.Normalize(localTargetPos);
                    // 回転角
                    float rotationDotProduct = Vector3.Dot(basis2Effector, basis2Target);
                    float rotationAngle = (float)Math.Acos(rotationDotProduct);

                    //回転量制限をかける
                    if (rotationAngle > MathHelper.Pi * ikBone.BoneData.IK.ControlWeight * i)
                        rotationAngle = MathHelper.Pi * ikBone.BoneData.IK.ControlWeight * i;
                    if (rotationAngle < -MathHelper.Pi * ikBone.BoneData.IK.ControlWeight * i)
                        rotationAngle = -MathHelper.Pi * ikBone.BoneData.IK.ControlWeight * i;
                    
                    if(!float.IsNaN(rotationAngle))
                    {
                        // 回転軸
                        Vector3 rotationAxis;
                        rotationAxis= Vector3.Cross(basis2Effector, basis2Target);
                        rotationAxis.Normalize();
                        //ボーンを修正
                        if (!CheckNaN(rotationAxis))
                            manager[bones[i]].BoneTransform = new QuatTransform(Quaternion.CreateFromAxisAngle(rotationAxis, rotationAngle), Vector3.Zero) * manager[bones[i]].BoneTransform;
#if DEBUG
                        CheckNaN(manager[bones[i]].BoneTransform);
#endif
                        manager[bones[i]].BoneTransform = limitation.AdjustTotalBoneMove(bones[i], manager);
#if DEBUG
                        CheckNaN(manager[bones[i]].BoneTransform);
#endif                        
                    }

                }
                /*if ((localEffectorPos - localTargetPos).LengthSquared() < errToleranceSq)
                {
                    return;
                }*/
            }
        }
        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;
        }
        internal static void CheckNaN(QuatTransform input)
        {
            if (float.IsNaN(input.Rotation.X))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Rotation.Y))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Rotation.Z))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Rotation.W))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Translation.X))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Translation.Y))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Translation.Z))
                throw new ApplicationException("NaNを発見");

        }
        internal static void CheckNaN2(Quaternion input)
        {
            if (float.IsNaN(input.X))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Y))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.Z))
                throw new ApplicationException("NaNを発見");
            if (float.IsNaN(input.W))
                throw new ApplicationException("NaNを発見");
        }
    }
    //設計途中で、MMDの場合、毎回のループで可動制限のためボーンに適応しなければならず、計算量が変わらないことに気づいたのでボツ
    /*internal class IK_ParticleSolver : IKSolver
    {
        struct Particle
        {
            public Matrix mat;//パーティクルの位置及び回転を表す
            public float constraint;//点と次の点との距離
        }
        Particle[] Particles = new Particle[100];//100個用意しておけば……(；・∀・)ﾀﾞ､ﾀﾞｲｼﾞｮｳﾌﾞ…?

        /// <summary>
        /// ソルバの種別
        /// </summary>
        public IKSolveType Type { get { return IKSolveType.CyclicCoordinateDescent; } }
        /// <summary>
        /// Particle法によるソルブ
        /// </summary>
        /// <param name="Movement">IKの位置(モデル全体の座標系)</param>
        /// <param name="bones">IK影響下のリスト(IKTargetからIK影響外の親まで逆順で)</param>
        /// <param name="ikBone">IKボーン</param>
        /// <param name="manager">ボーンマネージャ</param>
        /// <param name="limitation">IK回転方向制限オブジェクト</param>
        public void Solve(Vector3 Movement, List<int> bones, MMDBone ikBone, MMDBoneManager manager, IKLimitation limitation)
        {
            //再帰回数を取得
            ushort MaxIterations = ikBone.BoneData.IK.Iteration;
            //IKでソルブしない固定点を取得
            Matrix IKBase = manager.GetWorldQuatTransform(manager[bones.Last()].BoneData.SkeletonHierarchy).CreateMatrix();
            
            //パーティクルの計算
            for (ushort it = 0; it < MaxIterations; it++)
            {
                //パーティクルの更新
                Particles[bones.Count - 1].mat = manager[bones.Last()].BoneTransform.CreateMatrix() * IKBase;
                for (int i = bones.Count - 2; i >= 0; i--)
                {
                    Particles[i].mat = manager[bones[i]].BoneTransform.CreateMatrix() * Particles[i + 1].mat;
                    Particles[i].constraint = (Particles[i].mat.Translation - Particles[i + 1].mat.Translation).Length();
                }
                Vector3 dv = Movement - Particles[0].mat.Translation;
                Particles[0].mat.Translation += dv;

                for (int i = 1; i < bones.Count - 1; i++)
                {
                    dv = Particles[i].mat.Translation - Particles[i - 1].mat.Translation;
                    dv *= 0.5f - Particles[i - 1].constraint / dv.Length() * 0.5f;
                    Particles[i - 1].mat.Translation += dv;
                    Particles[i].mat.Translation -= dv;
                }
                dv = IKBase.Translation - Particles[bones.Count - 1].mat.Translation;
                dv *= 1f - Particles[bones.Count - 1].constraint / dv.Length() * 0.5f;
                Particles[Particles.Length - 1].mat.Translation += dv;
            }
            manager[bones[bones.Count - 1]].BoneTransform = QuatTransform.FromMatrix(Particles[bones.Count - 1].mat * Matrix.Invert(IKBase));
            //パーティクルのボーンへの当てはめ
            for (int i = 1; i <bones.Count; i++)
            {
                Matrix invCoord = Matrix.Invert(manager[bones[i-1]].BoneTransform.CreateMatrix());
                Vector3 p1 = Vector3.Transform(Particles[i].mat.Translation, invCoord);
                Vector3 p0 = Vector3.Transform(manager[bones[i]].BoneTransform.Translation, invCoord);
                Vector3 dv = Vector3.Normalize(p1 - p0);
                
                manager[bones[i]].BoneTransform = QuatTransform.FromMatrix(Particles[i].mat * Matrix.Invert(Particles[i + 1].mat));
                manager[bones[i]].BoneTransform = limitation.AdjustTotalBoneMove(bones[i], manager);
            }
        }
    }*/
}
