using Microsoft.Xna.Framework;
using BulletMMD.BulletDynamics.ConstraintSolver;

namespace MikuMikuDance.XNA.Model
{
    /// <summary>
    /// MMDからBulletのConstraintを使いやすくするためのラッパーライブラリ
    /// 現在コメントアウト
    /// </summary>
    class MMDJoint
    {
        //拘束(MikuMikuDanceはGeneric6DofSpringConstraintを使ってる)
        btGeneric6DofSpringConstraint mConstPoint;
        
    //    //Point2PointConstraint mConstPoint;
    //    //float ExplordThreashold = float.MaxValue;
    //    const float thresholdFactor=5.0f;
    //    //Vector3 defaultTransA, defaultTransB;
    //    //RigidBody BodyA, BodyB;
    //    //Quaternion defaultRotA, defaultRotB;
    //    /// <summary>
    //    /// default constructor so you can initialise this joint later
    //    /// </summary>
        public MMDJoint()
        {
            mConstPoint = new btGeneric6DofSpringConstraint();
        }

        
    //    public void Initialize(RigidBody body0, RigidBody body1, ModelData.MMDJoint joint,DynamicsWorld world)
    //    {
    //        Matrix frameInA, frameInB;
    //        Matrix jointPos = Matrix.CreateFromYawPitchRoll(joint.Rotation[1], joint.Rotation[0], joint.Rotation[2])
    //            * Matrix.CreateTranslation(joint.Position[0], joint.Position[1], joint.Position[2]);
    //        frameInA = jointPos * Matrix.Invert(BulletX.MathHelper.GetDisplayMatrix(body0.WorldTransform));
    //        frameInB = jointPos * Matrix.Invert(BulletX.MathHelper.GetDisplayMatrix(body1.WorldTransform));


    //        /*
    //        frameInA = Matrix.CreateFromYawPitchRoll(joint.ConstrainRotation1[1], joint.ConstrainRotation1[0], joint.ConstrainRotation1[2])
    //            * Matrix.CreateTranslation(joint.ConstrainPosition1[0], joint.ConstrainPosition1[1], joint.ConstrainPosition1[2]);
    //        frameInB = Matrix.CreateFromYawPitchRoll(joint.ConstrainRotation2[1], joint.ConstrainRotation2[0], joint.ConstrainRotation2[2])
    //                        * Matrix.CreateTranslation(joint.ConstrainPosition2[0], joint.ConstrainPosition2[1], joint.ConstrainPosition2[2]);
    //        */
            
    //        body0.ActivationState = ActivationState.DisableDeactivation;
    //        body1.ActivationState = ActivationState.DisableDeactivation;
    //        mConstPoint = new Generic6DofConstraint(body0, body1, frameInA, frameInB);
    //        //mConstPoint = new Point2PointConstraint(body0, body1, frameInA.Translation, frameInB.Translation);
    //        //G6Dof設定用変数の準備
    //        Vector3 minDist = MMDMath.VectorFromArray(joint.ConstrainPosition1);
    //        Vector3 maxDist = MMDMath.VectorFromArray(joint.ConstrainPosition2);
    //        Vector3 minRotate = MMDMath.VectorFromArray(joint.ConstrainRotation1);
    //        Vector3 maxRotate = MMDMath.VectorFromArray(joint.ConstrainRotation2);
    //        /*MMDMath.CheckMinMax(ref minDist, ref maxDist);
    //        MMDMath.CheckMinMax(ref minRotate, ref maxRotate);*/
    //        //Spring対応したら実装
    //        //Vector3 springMove = MMDMath.VectorFromArrow(joint.SpringPosition);
    //        //Vector3 springRotate = MMDMath.VectorFromArrow(joint.SpringRotation);
    //        System.Console.WriteLine(joint.Name + minDist.ToString() + maxDist.ToString() + minRotate.ToString() + maxRotate.ToString());

    //        mConstPoint.SetLinearLowerLimit(minDist);
    //        mConstPoint.SetLinearUpperLimit(maxDist);
    //        mConstPoint.SetAngularLowerLimit(minRotate);
    //        mConstPoint.SetAngularUpperLimit(maxRotate);
    //        /*body0.AngularFactor = 0.5f;
    //        body1.AngularFactor = 0.5f;*/
    //        /*mConstPoint.SetAngularLowerLimit(new Vector3(10, 10, 10));
    //        mConstPoint.SetAngularUpperLimit(new Vector3(0, 0, 0));*/
    //        /*mConstPoint.SetLinearLowerLimit(-Vector3.One*10);
    //        mConstPoint.SetLinearUpperLimit(new Vector3(10, 10, 10));*/


    //        world.AddConstraint(mConstPoint);


    //        /*ExplordThreashold = thresholdFactor * System.Math.Max(1.0f, System.Math.Max(MMDMath.VectorFromArray(joint.ConstrainPosition1).Length(), MMDMath.VectorFromArray(joint.ConstrainPosition2).Length()));
    //        Vector3 temp;
    //        body0.WorldTransform.Decompose(out temp,out defaultRotA,out defaultTransA);
    //        body1.WorldTransform.Decompose(out temp,out defaultRotB,out defaultTransB);
    //        BodyA = body0;
    //        BodyB = body1;*/
    //    }

    //    //原因不明の拘束爆発現象の抑制用……に作ったがうまくいかず
    //    public void RestorExplosion()
    //    {
    //        //if ((BodyA.WorldTransform.Translation - BodyB.WorldTransform.Translation).Length()
    //        //    -(defaultTransA-defaultTransB).Length()
    //        //    > ExplordThreashold)
    //        //{//爆発したらリセット……せこいかなぁ……
    //            //mConstPoint.RestoreExplosion();
    //            /*if (!BodyA.IsStaticObject)
    //                {
    //                    BodyA.WorldTransform = Matrix.CreateFromQuaternion(defaultRotA) * Matrix.CreateTranslation(defaultTransA);
    //                    BodyA.LinearVelocity = Vector3.Zero;
    //                    BodyA.AngularVelocity = Vector3.Zero;
    //                    DefaultMotionState dms = BodyA.MotionState as DefaultMotionState;
    //                    if (dms != null)
    //                        dms.SetWorldTransform(Matrix.CreateFromQuaternion(defaultRotA) * Matrix.CreateTranslation(defaultTransA));
    //                }
    //                if (!BodyB.IsStaticObject)
    //                {
    //                    BodyB.WorldTransform = Matrix.CreateFromQuaternion(defaultRotB) * Matrix.CreateTranslation(defaultTransB);
    //                    BodyB.LinearVelocity = Vector3.Zero;
    //                    BodyB.AngularVelocity = Vector3.Zero;
    //                    DefaultMotionState dms = BodyB.MotionState as DefaultMotionState;
    //                    if (dms != null)
    //                        dms.SetWorldTransform(Matrix.CreateFromQuaternion(defaultRotB) * Matrix.CreateTranslation(defaultTransB));
                
    //                }*/
    //        //}
    //    }

    }
}
