using System;
using BulletX.BulletCollision.CollisionDispatch;
using BulletX.BulletCollision.CollisionShapes;
using BulletX.BulletDynamics.ConstraintSolver;
using BulletX.BulletDynamics.Dynamics;
using BulletX.LinerMath;
using MikuMikuDance.XNA.Model.ModelData;
using MikuMikuDance.XNA.MultiThread;

namespace MikuMikuDance.XNA.Model
{
    /// <summary>
    /// 物理マネージャー
    /// </summary>
    public class MMDPhysicsManager
    {
        /// <summary>
        /// 剛体
        /// </summary>
        public RigidBody[] Rigids { get; private set; }
        internal MMDMotionState[] motionState;
        MMDModel model;
        PhysicsThread m_thread;
        /// <summary>
        /// ジョイント
        /// </summary>
        public Generic6DofSpringConstraint[] Joints { get; private set; }

        internal void Setup(MMDModelData modelData, MMDModel Model, DiscreteDynamicsWorld world, PhysicsThread thread)
        {
            model = Model;
            m_thread = thread;
            //初期化
            Rigids = new RigidBody[modelData.Rigids.Length];
            motionState = new MMDMotionState[modelData.Rigids.Length];
            Joints = new Generic6DofSpringConstraint[modelData.Joints.Length];
            //剛体の作成
            for (int i = 0; i < Rigids.Length; i++)
            {
                Rigids[i] = CreateRigidBody(modelData.Rigids[i], Model, world, thread, out motionState[i]);
            }
            //ジョイント(拘束)の作成
            for (int i = 0; i < Joints.Length; i++)
            {
                RigidBody phisic0 = Rigids[(int)modelData.Joints[i].RigidBodyA];
                RigidBody phisic1 = Rigids[(int)modelData.Joints[i].RigidBodyB];
                Joints[i] = CreateJoint(phisic0, phisic1, modelData.Joints[i], world);
            }
        }

        internal void Update()
        {
            for (int i = 0; i < motionState.Length; i++)
                motionState[i].Flush(false);
        }
        /// <summary>
        /// 剛体のリセット(BoneUpdate無し)
        /// </summary>
        public void ResetRigidWithoutBoneUpdate()
        {
            if (m_thread == null)
            {
                for (int i = 0; i < motionState.Length; i++)
                {
                    Rigids[i].activate(true);
                    motionState[i].Flush(true);
                    btTransform temp;
                    motionState[i].getWorldTransform(out temp);
                    Rigids[i].WorldTransform = temp;
                    Rigids[i].LinearVelocity = btVector3.Zero;
                    Rigids[i].AngularVelocity = btVector3.Zero;
                }
            }
            else
            {
                m_thread.FreeBuffer.ResetRigid(model);
            }
        }
        /// <summary>
        /// 剛体のリセット
        /// </summary>
        public void ResetRigid()
        {
            if (m_thread == null)
            {
                model.BoneManager.UpdateWorldTransforms();
                for (int i = 0; i < motionState.Length; i++)
                {
                    Rigids[i].activate(true);
                    motionState[i].Flush(true);
                    btTransform temp;
                    motionState[i].getWorldTransform(out temp);
                    Rigids[i].WorldTransform = temp;
                    Rigids[i].LinearVelocity = btVector3.Zero;
                    Rigids[i].AngularVelocity = btVector3.Zero;
                }
            }
            else
            {
                m_thread.FreeBuffer.ResetRigid(model);
            }
        }

        private RigidBody CreateRigidBody(MMDRigid rigid, MMDModel Model, DiscreteDynamicsWorld world, PhysicsThread thread, out MMDMotionState motionStateStart)
        {
            CollisionShape collision;
            RigidBody body;
            //衝突スキンの作成
            switch (rigid.ShapeType)
            {
                case 0:
                    collision = new SphereShape(rigid.ShapeWidth);
                    break;
                case 1:
                    collision = new BoxShape(new btVector3(rigid.ShapeWidth, rigid.ShapeHeight, rigid.ShapeDepth));
                    break;
                case 2:
                    collision = new CapsuleShape(rigid.ShapeWidth, rigid.ShapeHeight);
                    break;
                default:
                    throw new NotImplementedException("不明な剛体タイプ");
            }


            motionStateStart = new MMDMotionState(rigid, Model, thread);

            btVector3 localInertia = btVector3.Zero;
            //イナーシャの計算
            if (rigid.Type != 0)
                collision.calculateLocalInertia(rigid.Weight, out localInertia);

            
            //剛体を作成
            body = new RigidBody((rigid.Type != 0 ? rigid.Weight : 0), motionStateStart, collision, localInertia);
            //ダンピング値、摩擦、Restitutionをセット
            body.setDamping(rigid.LinerDamping, rigid.AngularDamping);
            body.Friction = rigid.Friction;
            body.Restitution = rigid.Restitution;

            //ボーン追従型はKinematicにする
            if (rigid.Type == 0)
            {
                body.ActivationState |= ActivationStateFlags.DISABLE_DEACTIVATION;
                body.CollisionFlags = CollisionFlags.CF_KINEMATIC_OBJECT;
                if (rigid.RelatedBoneIndex != 0xffff)
                    model.BoneManager.Bones[rigid.RelatedBoneIndex].IsPhysics = false;
            }
            else
            {//物理演算型はボーンのフラグをオンにする
                if (rigid.RelatedBoneIndex != 0xffff)
                    model.BoneManager.Bones[rigid.RelatedBoneIndex].IsPhysics = true;
            }
            //グループのフラグをセット
            short group = (short)Math.Pow(2, rigid.GroupIndex);

            //ボディを有効化し、グループとマスクを適応
            world.addRigidBody(body, group, (short)rigid.GroupTarget);
            
            return body;
        }
        private Generic6DofSpringConstraint CreateJoint(RigidBody body0, RigidBody body1, MikuMikuDance.XNA.Model.ModelData.MMDJoint joint, DiscreteDynamicsWorld world)
        {
            Microsoft.Xna.Framework.Matrix frameInA, frameInB;
            Microsoft.Xna.Framework.Matrix jointPos = Microsoft.Xna.Framework.Matrix.CreateFromYawPitchRoll(joint.Rotation[1], joint.Rotation[0], joint.Rotation[2])
                * Microsoft.Xna.Framework.Matrix.CreateTranslation(joint.Position[0], joint.Position[1], joint.Position[2]);
            if (body0.MotionState != null)
            {
                MMDMotionState motionState = (MMDMotionState)body0.MotionState;
                motionState.GraphicsWorldTrans.getXNAMatrix(out frameInA);
            }
            else
                throw new NotImplementedException("来るハズないのだが");
            frameInA = jointPos * model.Transform.CreateMatrix() * Microsoft.Xna.Framework.Matrix.Invert(frameInA);
            if (body1.MotionState != null)
            {
                MMDMotionState motionState = (MMDMotionState)body1.MotionState;
                motionState.GraphicsWorldTrans.getXNAMatrix(out frameInB);
            }
            else
                throw new NotImplementedException("来るハズないのだが");
            frameInB = jointPos * model.Transform.CreateMatrix() * Microsoft.Xna.Framework.Matrix.Invert(frameInB);
            //frameInB = jointPos * Matrix.Invert(MMDMath.ConvertToMatrix(body1.GetWorldTransformSmart()));
            Generic6DofSpringConstraint mConstPoint = new Generic6DofSpringConstraint(body0, body1, new btTransform(ref frameInA), new btTransform(ref frameInB), true);

            //G6Dof設定用変数の準備
            mConstPoint.setLinearLowerLimit(new btVector3(joint.ConstrainPosition1));
            mConstPoint.setLinearUpperLimit(new btVector3(joint.ConstrainPosition2));
            mConstPoint.setAngularLowerLimit(new btVector3(joint.ConstrainRotation1));
            mConstPoint.setAngularUpperLimit(new btVector3(joint.ConstrainRotation2));
            for (int i = 0; i < 3; i++)
            {
                mConstPoint.setStiffness(i, joint.SpringPosition[i]);
                mConstPoint.enableSpring(i, true);
                mConstPoint.setStiffness(i + 3, joint.SpringRotation[i]);
                mConstPoint.enableSpring(i + 3, true);
            }
            mConstPoint.calculateTransforms();
            mConstPoint.setEquilibriumPoint();
            
            world.addConstraint(mConstPoint);
            return mConstPoint;
        }

        
    }
}
