#include "RigidBody.h"
#include "BulletCLIMath.h"
#include "BulletDynamics/Dynamics/DiscreteDynamicsWorld.h"

namespace BulletCLI
{
#pragma unmanaged
    btRigidBody* CreateRigidBody(float mass, btDefaultMotionState* motionState, btCollisionShape* collisionShape,btVector3& localInertia)
    {
        return new ::btRigidBody(mass,motionState,collisionShape,localInertia);
    }
#pragma managed
	RigidBody::RigidBody(float mass, DefaultMotionState^ motionState, ICollisionShape^ collisionShape,Vector3 localInertia)
	{
        inertia=BulletCLI::XVectorToBTVector(localInertia);
        this->m_p=BulletCLI::CreateRigidBody(
            mass,
            motionState->GetPtr(),
            collisionShape->GetPtr(),
            *inertia);
        delete this->inertia;
        m_bRegisterd=false;
	}
	RigidBody::~RigidBody()
	{
		this->!RigidBody();
	}
	RigidBody::!RigidBody()
	{
        if(!m_bRegisterd)
        {
            SAFE_DELETE(this->m_p);
        }
	}
    void RigidBody::SetCollisionFlags(CollisionFlags flag)
    {
        this->m_p->setCollisionFlags((int)flag);
    }
    void RigidBody::SetDamping(float lin_damping, float ang_damping)
    {
        btScalar lin(lin_damping);
        btScalar ang(ang_damping);
        this->m_p->setDamping(lin,ang);
    }
    void RigidBody::SetFriction(float friction)
    {
        btScalar fric(friction);
        this->m_p->setFriction(friction);
    }
    void RigidBody::SetRestitution(float restitution)
    {
        btScalar rest(restitution);
        this->m_p->setRestitution(rest);
    }
    void RigidBody::SetActivationState(int state)
    {
        this->m_p->setActivationState(state);
    }
    int RigidBody::GetActivationState()
    {
        return this->m_p->getActivationState();
    }
    Matrix RigidBody::GetWorldTransform()
    {
        Matrix result;
        BulletCLI::BTTransformToXMatrix(this->m_p->getWorldTransform(),result);
        return result;
    }
    Matrix RigidBody::GetWorldTransformSmart()
    {
        Matrix result;
        //本当はここでgetCollisionObjectArrayからcollisionObj取得→bodyにUpcastする必要がある
        //がここでは確実にBodyがあるのでパス
        if(m_p->getMotionState())
        {
            btTransform* transform=NULL;
            btDefaultMotionState* motionState=(btDefaultMotionState*)m_p->getMotionState();
            transform=&motionState->m_graphicsWorldTrans;
            BulletCLI::BTTransformToXMatrix(*transform,result);
        }
        else
        {
            btTransform* transform=new btTransform();
            btMotionState* motionState=this->m_p->getMotionState();
            motionState->getWorldTransform(*transform);
            BulletCLI::BTTransformToXMatrix(*transform,result);
            delete transform;
        }
        return result;
    }
#pragma unmanaged
    void CopyTransform(btTransform& t1,const btTransform& t2)
    {
        t1=t2;
    }
#pragma managed
    void RigidBody::SetWorldTransformByMotionState(Matrix^ matrix)
    {
        //上参照
        if(m_p->getMotionState())
        {
            btTransform* transform;
            transform=BulletCLI::XMatrixToBTTransform(matrix);
            btDefaultMotionState* motionState=(btDefaultMotionState*)m_p->getMotionState();
            CopyTransform(motionState->m_graphicsWorldTrans,*transform);
            delete transform;
        }
        else
        {
            btTransform* transform;
            transform=BulletCLI::XMatrixToBTTransform(matrix);
            btMotionState* motionState=this->m_p->getMotionState();
            motionState->setWorldTransform(*transform);
            delete transform;
        }
    }
    void RigidBody::SetWorldTransform(Matrix^ matrix)
    {
        
        btTransform* transform;
        transform=BulletCLI::XMatrixToBTTransform(matrix);
        //btMotionState* motionState=this->m_p->getMotionState();
        //motionState->setWorldTransform(*transform);
        m_p->setWorldTransform(*transform);
        delete transform;
        
    }
    void RigidBody::SetDeactivationTime(float time)
    {
        btScalar t(time);
        this->m_p->setDeactivationTime(t);
    }
    void RigidBody::Activate(bool forceActivation)
    {
        this->m_p->activate(forceActivation);
        this->m_p->updateDeactivation(10);
    }
    void RigidBody::ForceActivationState(int state)
    {
        this->m_p->forceActivationState(state);
    }
    void RigidBody::ResetVelocity()
    {
        btVector3* zero=new btVector3(0,0,0);
        this->m_p->setLinearVelocity(*zero);
        this->m_p->setAngularVelocity(*zero);
        delete zero;
    }
}