using Microsoft.Xna.Framework;
//using BulletCLI;
using MikuMikuDance.XNA.Model.ModelData;
using BulletX.BulletDynamics.ConstraintSolver;
using BulletX.BulletDynamics.Dynamics;
using BulletX.LinerMath;
using System;

namespace MikuMikuDance.XNA.Model
{
    /// <summary>
    /// MMDXからBulletのConstraintを使いやすくするためのラッパーライブラリ
    /// </summary>
    class MMDPhysicJoint
    {
        //拘束(MikuMikuDanceはGeneric6DofSpringConstraintを使ってる)
        Generic6DofSpringConstraint mConstPoint;
        //外部から作成不可
        internal MMDPhysicJoint(RigidBody body0, RigidBody body1, MMDJointData joint, DiscreteDynamicsWorld 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]);
            if (body0.MotionState != null)
            {
                DefaultMotionState motionState = (DefaultMotionState)body0.MotionState;
                motionState.m_graphicsWorldTrans.getXNAMatrix(out frameInA);
            }
            else
                throw new NotImplementedException("来るハズないのだが");
            frameInA = jointPos * Matrix.Invert(frameInA);
            //frameInA = jointPos * Matrix.Invert(MMDMath.ConvertToMatrix(body0.GetWorldTransformSmart()));
            if (body1.MotionState != null)
            {
                DefaultMotionState motionState = (DefaultMotionState)body1.MotionState;
                motionState.m_graphicsWorldTrans.getXNAMatrix(out frameInB);
            }
            else
                throw new NotImplementedException("来るハズないのだが");
            frameInB = jointPos * Matrix.Invert(frameInB);
            //frameInB = jointPos * Matrix.Invert(MMDMath.ConvertToMatrix(body1.GetWorldTransformSmart()));
            mConstPoint = new Generic6DofSpringConstraint(body0, body1, new btTransform(ref frameInA), new btTransform(ref frameInB), true);

            //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);
            */
            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);
            }

            world.addConstraint(mConstPoint);
        }

    }
}
