using System;
using MikuMikuDance.XNA.Model.ModelData;
using Microsoft.Xna.Framework;
//using BulletX.Dynamics;
//using BulletX;

namespace MikuMikuDance.XNA.Model
{
    /// <summary>
    /// カプセル型剛体クラス
    /// </summary>
    class MMDPhisicsCapsule : MMDPhisicsObject
    {
    //    public MMDPhisicsCapsule(MMDRigid rigid, MMDModel model, DynamicsWorld world)
    //        : base(rigid, model)
    //    {
    //        //衝突スキンの作成
    //        collision = new CapsuleShape(rigid.ShapeWidth, rigid.ShapeHeight);

    //        //初期の姿勢を計算
    //        Matrix startTransform;
    //        if (rigid.RelatedBoneIndex != 0xffff)
    //            startTransform = model.BoneManager[rigid.RelatedBoneIndex].WorldTransform.CreateMatrix();
    //        else
    //        {
    //            if (model.BoneManager.ContainsBone("センター"))
    //                startTransform = model.BoneManager["センター"].WorldTransform.CreateMatrix();
    //            else
    //                startTransform = Matrix.Identity;
    //        }
    //        startTransform = BulletX.MathHelper.GetDisplayMatrix(CreateOrientation(rigid) * startTransform * Model.World);

    //        //初期の姿勢をMotionStateに設定
    //        DefaultMotionState motionState = new DefaultMotionState(startTransform, Matrix.Identity);
    //        Vector3 localInertia = Vector3.Zero;
    //        //イナーシャの計算
    //        if (rigid.Type != 0)
    //            collision.CalculateLocalInertia(rigid.Weight, out localInertia);
    //        //関連ボーン番号を取得しておく
    //        RelatedBone = rigid.RelatedBoneIndex;
            
    //        //if(rigid.Name=="右髪6")
    //        //剛体を作成
    //            body = new RigidBody((rigid.Type != 0 ? rigid.Weight : 0), motionState, collision, localInertia,
    //                rigid.LinerDamping, rigid.AngularDamping, rigid.Friction, rigid.Restitution);
    //       /* else
    //            body = new RigidBody(0, motionState, collision, localInertia,
    //                rigid.LinerDamping, rigid.AngularDamping, rigid.Friction, rigid.Restitution);*/
            
    //        //グループのフラグを作成
    //        ushort group = (ushort)Math.Pow(2, Rigid.GroupIndex);

    //        //ボディを有効化し、グループとマスクをセット
    //        world.AddRigidBody(body, group, rigid.GroupTarget);
    //    }
    //    internal override void ApplyRigitToBone()
    //    {
    //        if (RelatedBone == 0xffff)
    //            return;
    //        if (Rigid.Type != 0 && Rigid.RelatedBoneIndex < Model.BoneManager.Count)
    //        {
    //            //メモ：剛体の回転は対象ボーンの全ての子に、移動は対象ボーンに適応する
    //            //方法：親のボーンの回転と移動を適応する。適応する時にボーン座標系に直す
    //            //まずは対象ボーンの親の座標変換を取得する
    //            QuatTransform transform = new QuatTransform(Quaternion.Identity, Vector3.Zero);
    //            if (Model.BoneManager[Rigid.RelatedBoneIndex].BoneData.SkeletonHierarchy >= 0)
    //                transform = Model.BoneManager.GetWorldQuatTransform(Model.BoneManager[Rigid.RelatedBoneIndex].BoneData.SkeletonHierarchy);

    //            Matrix rmat = BulletX.MathHelper.GetDisplayMatrix(((DefaultMotionState)body.MotionState).GraphicsWorldTransform);
    //            rmat = Matrix.Invert(CreateOrientation(Rigid)) * rmat * Matrix.Invert(Model.World);

    //            //Matrix rmat = body.WorldTransform * Matrix.Invert(Model.World) * Matrix.Invert(CreateOrientation(Rigid));
    //            Model.BoneManager[Rigid.RelatedBoneIndex].WorldTransform = QuatTransform.FromMatrix(rmat);
    //            Model.BoneManager[Rigid.RelatedBoneIndex].BoneTransform = QuatTransform.FromMatrix(rmat * Matrix.Invert(transform.CreateMatrix()));

    //            if (Rigid.Name == "右髪6")
    //                Console.WriteLine(body.LinearVelocity.ToString());
                
    //            //QuatTransform transform = new QuatTransform(Quaternion.Identity, Vector3.Zero);
    //            //if (Model.BoneManager[Rigid.RelatedBoneIndex].BoneData.SkeletonHierarchy >= 0)
    //            //    transform = Model.BoneManager.GetWorldQuatTransform(Model.BoneManager[Rigid.RelatedBoneIndex].BoneData.SkeletonHierarchy);
    //            ////それを元に回転と移動を計算する
    //            ////カプセル→ボーンには補正が必要。カプセルの中心＝ボーンの真ん中だから

    //            ////ワールド移動の計算
    //            //Vector3 pos = body.Position;// -transform.Translation;
    //            //Matrix rot = body.Orientation;// *Matrix.Invert(transform.CreateMatrix());
    //            //Matrix defaultOrientation =  CreateOrientation(Rigid);
    //            ////ボックスと同じ補正を当ててみたらうまくいった
    //            //pos += -Vector3.Transform(new Vector3(Rigid.Position[0], Rigid.Position[1], Rigid.Position[2]), body.Orientation);/*new Vector3(0,3,0)*/ //Vector3.Transform(new Vector3(0, Rigid.ShapeHeight/2, 0), rot * Matrix.Invert(defaultOrientation));
    //            //Quaternion rotation;
    //            //Vector3 temp1, temp2;
    //            //rot.Decompose(out temp1, out rotation, out temp2);
    //            //rotation.Normalize();
    //            //Model.BoneManager[Rigid.RelatedBoneIndex].WorldTransform = new QuatTransform(rotation, pos);
    //            ////ボーン座標でのボーンの移動
    //            //Matrix bonemat = (new QuatTransform(rotation, pos).CreateMatrix()) * Matrix.Invert(transform.CreateMatrix());
    //            //Model.BoneManager[Rigid.RelatedBoneIndex].BoneTransform = QuatTransform.FromMatrix(bonemat);
    //        }
    //        base.ApplyRigitToBone();
    //    }
    //    internal override void ApplyBoneToRigit()
    //    {
    //        if (RelatedBone == 0xffff)
    //            return;
    //        if (Rigid.Type == 0 && Rigid.RelatedBoneIndex < Model.BoneManager.Count)
    //        {
    //            Matrix bone = Model.BoneManager[Rigid.RelatedBoneIndex].WorldTransform.CreateMatrix();
    //            bone = BulletX.MathHelper.GetDisplayMatrix(CreateOrientation(Rigid) * bone * Model.World);
    //            body.MotionState.SetWorldTransform(bone);
    //            body.WorldTransform = bone;

    //            //メモ：この段階ですでにボーンのワールドのトランスフォームが確定しているので
    //            //ボーンの座標から生成してく
    //            /*QuatTransform bone = Model.BoneManager[Rigid.RelatedBoneIndex].WorldTransform;
    //            body.Position = bone.Translation + Vector3.Transform(new Vector3(Rigid.Position[0], Rigid.Position[1], Rigid.Position[2]), body.Orientation);
    //            body.Orientation = Matrix.CreateFromQuaternion(bone.Rotation);*/
    //        }
    //        base.ApplyBoneToRigit();
    //    }
    }
}
