using System;
using System.Collections.Generic;
//using BulletCLI;
using Microsoft.Xna.Framework;
using MikuMikuDance.XNA.Model.ModelData;
using BulletX.BulletCollision.CollisionShapes;
using BulletX.BulletDynamics.Dynamics;
using BulletX.LinerMath;
using BulletX.BulletCollision.CollisionDispatch;

namespace MikuMikuDance.XNA.Model
{
    /// <summary>
    /// MMDXからBulletの剛体を使いやすくするためのラッパーライブラリ
    /// </summary>
    class MMDRigidBody
    {
        public RigidBody body { get; private set; }
        public CollisionShape collision { get; private set; }
        public MMDRigidData Rigid { get; private set; }
        public MMDModel Model { get; private set; }
        public UInt16 RelatedBone { get; private set; }
        DefaultMotionState motionStateStart;//CLIのGCで消されないよう
        Matrix RigidBias;
        //外部から作成不可
        internal MMDRigidBody(MMDRigidData rigid, MMDModel model, DiscreteDynamicsWorld world)
        {
            Model = model;
            Rigid = rigid;
            //衝突スキンの作成
            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("不明な剛体タイプ");
            }
            
            //初期の姿勢を計算
            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 = Matrix.Identity;
            }
            //ボーンと剛体とのズレを計算
            RigidBias = CreateRigidBias(rigid);
            //初期姿勢を計算
            startTransform = RigidBias * startTransform * Model.World;
            //初期の姿勢をMotionStateに設定
            btTransform startTrans=new btTransform(ref startTransform);
            motionStateStart = new DefaultMotionState(startTrans, btTransform.Identity);

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

            //関連ボーン番号を取得しておく
            RelatedBone = rigid.RelatedBoneIndex;

            //剛体を作成
            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;
            }
            //グループのフラグをセット
            short group = (short)Math.Pow(2, Rigid.GroupIndex);

            //ボディを有効化し、グループとマスクを適応
            world.addRigidBody(body, group, (short)rigid.GroupTarget);
        }
        /*public static MMDPhysicsObject PhisicsObjectFactory(MMDRigidData rigid, MMDModel model, DiscreteDynamicsWorld world)
        {
            switch (rigid.ShapeType)
            {
                case 0://球
                    return new MMDPhysicsSphere(rigid, model, world);
                case 1://箱
                    return new MMDPhysicsBox(rigid, model, world);
                case 2://カプセル
                    return new MMDPhysicsCapsule(rigid, model, world);
                default:
                    throw new ApplicationException("Undefined Rigid Shape Type");
            }
        }*/

        /// <summary>
        /// 剛体→ボーンへの適応
        /// </summary>
        internal virtual void ApplyRigitToBone() 
        {
            if (RelatedBone == 0xffff)
                return;
            if (Rigid.Type != 0 && Rigid.RelatedBoneIndex < Model.BoneManager.Bones.Length)
            {
                Matrix RigidPos;
                if (body.MotionState != null)
                {
                    DefaultMotionState motionState = (DefaultMotionState)body.MotionState;
                    motionState.m_graphicsWorldTrans.getXNAMatrix(out RigidPos);
                }
                else
                {
                    throw new NotImplementedException("ここにはこないはずだが……");
                    /*btTransform bodyPosition;
                    body.MotionState.getWorldTransform(out bodyPosition);
                    bodyPosition.getXNAMatrix(out RigidPos);*/
                }
                Matrix temp1, temp2, temp3;
                Matrix.Invert(ref RigidBias, out temp1);
                Matrix.Multiply(ref temp1, ref RigidPos, out temp2);
                temp1 = Matrix.Invert(Model.World);
                Matrix.Multiply(ref temp2, ref temp1, out temp3);
                Model.BoneManager.Bones[Rigid.RelatedBoneIndex].WorldTransform = QuatTransform.FromMatrix(temp3);
                //Model.BoneManager.Bones[Rigid.RelatedBoneIndex].WorldTransform = QuatTransform.FromMatrix(Matrix.Invert(RigidBias) * RigidPos * Matrix.Invert(Model.World));
            }
        }
        /// <summary>
        /// ボーン→剛体への適応
        /// </summary>
        internal virtual void ApplyBoneToRigit() 
        {
            if (RelatedBone == 0xffff)
                return;
            if (Rigid.Type == 0 && Rigid.RelatedBoneIndex < Model.BoneManager.Bones.Length)
            {
                btTransform world;
                Matrix temp,temp2;
                temp = RigidBias * Model.BoneManager.Bones[Rigid.RelatedBoneIndex].WorldTransform.CreateMatrix();
                Matrix.Multiply(ref temp, ref Model.World, out temp2);
                world = new btTransform(ref temp2);
                if (body.MotionState != null)
                {
                    DefaultMotionState motionState = (DefaultMotionState)body.MotionState;
                    motionState.m_graphicsWorldTrans = world;
                }
                //body.SetWorldTransformByMotionState(world);
                //body.SetWorldTransformByMotionState(MMDMath.ConvertToBulletTransform(RigidBias *
                //    Model.BoneManager.Bones[Rigid.RelatedBoneIndex].WorldTransform.CreateMatrix() * Model.World));
            }
        }
        
        protected Matrix CreateRigidBias(MMDRigidData rigid)
        {
            return Matrix.CreateFromYawPitchRoll(rigid.Rotation[1],rigid.Rotation[0],rigid.Rotation[2])
                * Matrix.CreateTranslation(rigid.Position[0], rigid.Position[1], rigid.Position[2]);
        }
        internal void ResetRigid()
        {
            if (Rigid.RelatedBoneIndex != 0xffff)
            {
                body.activate(true);
                btTransform world;
                Matrix temp, temp2;
                temp = RigidBias * Model.BoneManager.Bones[Rigid.RelatedBoneIndex].WorldTransform.CreateMatrix();
                Matrix.Multiply(ref temp, ref Model.World, out temp2);
                world=new btTransform(ref temp2);
                if (body.MotionState != null)
                {
                    DefaultMotionState motionState = (DefaultMotionState)body.MotionState;
                    motionState.m_graphicsWorldTrans = world;
                }
                else
                    throw new NotImplementedException("来るハズないのだが……");
                //body.SetWorldTransformByMotionState(world);
                //body.SetWorldTransform(MMDMath.ConvertToBulletTransform(RigidBias *
                //            Model.BoneManager.Bones[Rigid.RelatedBoneIndex].WorldTransform.CreateMatrix() * Model.World));
                body.LinearVelocity = btVector3.Zero;
                body.AngularVelocity = btVector3.Zero;
            }
        }
    }
    class MMDPhisicsSorter : IComparer<MMDRigidBody>
    {
        public int Compare(MMDRigidBody o1, MMDRigidBody o2)
        {
            return o1.Rigid.RelatedBoneIndex - o2.Rigid.RelatedBoneIndex;
        }
    }
}
