using BulletX.LinerMath;
using MikuMikuDance.XNA.Model.ModelData;
using System.Threading;
using System;
using MikuMikuDance.XNA.MultiThread;

namespace MikuMikuDance.XNA.Model
{
    /// <summary>
    /// MikuMikuDance用のMotionState
    /// </summary>
    public class MMDMotionState : IMotionState
    {
        /// <summary>
        /// graphicsWorldTransform
        /// </summary>
        public btTransform GraphicsWorldTrans { get { return m_thread == null ? m_graphicsWorldTrans[0] : m_graphicsWorldTrans[m_thread.OutThread]; } }
        btTransform[] m_graphicsWorldTrans;
        btTransform m_centerOfMassOffset;
        btTransform m_startWorldTrans;
        MMDRigid m_rigid;
        Microsoft.Xna.Framework.Matrix m_rigidBias;
        MMDModel m_model;
        PhysicsThread m_thread;
        //関連ボーン番号を取得しておく
        ushort    RelatedBone;
        /// <summary>
        /// ユーザーデータ
        /// </summary>
        public object UserData { get; set; }
        /// <summary>
        /// デフォルトコンストラクタ
        /// </summary>
        /// <param name="rigid">MMD剛体データ</param>
        /// <param name="Model">MMDモデルデータ</param>
        /// <param name="thread">物理エンジン用スレッド</param>
        public MMDMotionState(MMDRigid rigid, MMDModel Model, PhysicsThread thread)
        {
           UserData = null;
            m_rigid = rigid;
            m_thread = thread;
            //初期の姿勢を計算
            Microsoft.Xna.Framework.Matrix startTransform;
            if (rigid.RelatedBoneIndex != 0xffff)
                startTransform = Model.BoneManager.Bones[rigid.RelatedBoneIndex].WorldTransform.CreateMatrix();
            else
            {
                if (Model.BoneManager.ContainsBone("センター"))
                    startTransform = Model.BoneManager.Bones[Model.BoneManager.IndexOf("センター")].WorldTransform.CreateMatrix();
                else
                    startTransform = Microsoft.Xna.Framework.Matrix.Identity;
            }
            //ボーンと剛体とのズレを計算
            Microsoft.Xna.Framework.Matrix RigidBias = CreateRigidBias(rigid);
            //初期姿勢を計算
            startTransform = RigidBias * startTransform * Model.Transform.CreateMatrix();
            //初期の姿勢をMotionStateに設定
            btTransform startTrans = new btTransform(ref startTransform);
            m_graphicsWorldTrans = new btTransform[thread != null ? 2 : 1];
            m_graphicsWorldTrans[0] = startTrans;
            if (thread != null)
                m_graphicsWorldTrans[1] = startTrans;
            m_centerOfMassOffset = btTransform.Identity;
            m_startWorldTrans = startTrans;
            //これからの計算ように確保
            m_rigidBias = RigidBias;
            RelatedBone = rigid.RelatedBoneIndex;
            m_model = Model;
        }
        //graphicsWorldTransの更新
        internal void Flush(bool reset)
        {
            if (RelatedBone >= m_model.BoneManager.Bones.Length)
                return;
            if (m_rigid.Type == 0 || reset)
            {
                Microsoft.Xna.Framework.Matrix temp, temp2, temp3=m_model.Transform.CreateMatrix();
                temp = m_rigidBias * m_model.BoneManager.Bones[RelatedBone].WorldTransform.CreateMatrix();
                Microsoft.Xna.Framework.Matrix.Multiply(ref temp, ref temp3, out temp2);
                if (!reset)
                    m_graphicsWorldTrans[m_thread != null ? m_thread.OutThread : 0] = new btTransform(ref temp2);
                else
                {
                    m_graphicsWorldTrans[0] = new btTransform(ref temp2);
                    if (m_thread != null)
                        m_graphicsWorldTrans[1] = m_graphicsWorldTrans[0];
                }
            }
            else
            {
                Microsoft.Xna.Framework.Matrix RigidPos;
                m_graphicsWorldTrans[m_thread != null ? m_thread.OutThread : 0].getXNAMatrix(out RigidPos);
                
                Microsoft.Xna.Framework.Matrix temp1, temp2, temp3;
                Microsoft.Xna.Framework.Matrix.Invert(ref m_rigidBias, out temp1);
                Microsoft.Xna.Framework.Matrix.Multiply(ref temp1, ref RigidPos, out temp2);
                temp1 = Microsoft.Xna.Framework.Matrix.Invert(m_model.Transform.CreateMatrix());
                Microsoft.Xna.Framework.Matrix.Multiply(ref temp2, ref temp1, out temp3);
                m_model.BoneManager.Bones[RelatedBone].WorldTransform = QuatTransform.FromMatrix(temp3);

            }

        }

        ///synchronizes world transform from user to physics
	    public virtual void	getWorldTransform(out btTransform centerOfMassWorldTrans ) 
	    {
                centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans[m_thread!=null?m_thread.OnThread:0];
            
	    }

	    ///synchronizes world transform from physics to user
	    ///Bullet only calls the update of worldtransform for active objects
        public virtual void setWorldTransform(btTransform centerOfMassWorldTrans)
        {
            m_graphicsWorldTrans[m_thread != null ? m_thread.OnThread : 0] = centerOfMassWorldTrans * m_centerOfMassOffset;

        }
        Microsoft.Xna.Framework.Matrix CreateRigidBias(MMDRigid rigid)
        {
            return Microsoft.Xna.Framework.Matrix.CreateFromYawPitchRoll(rigid.Rotation[1], rigid.Rotation[0], rigid.Rotation[2])
                * Microsoft.Xna.Framework.Matrix.CreateTranslation(rigid.Position[0], rigid.Position[1], rigid.Position[2]);
        }

        
    }
}