#include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
#include <BulletCollision/CollisionDispatch/btCollisionDispatcher.h>
#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
#include <BulletCollision/CollisionShapes/btBoxShape.h>
#include <LinearMath/btMotionState.h>
#include <LinearMath/btDefaultMotionState.h>
#include <iostream>
#include <unistd.h>

int main()
{
    // Build the broadphase
    btBroadphaseInterface* broadphase = new btDbvtBroadphase();

    // Set up the collision configuration and dispatcher
    btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
    btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);

    // The actual physics solver
    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

    // The world.
    btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);
    dynamicsWorld->setGravity(btVector3(0,-9.81f,0));

    btCollisionShape* boxCollisionShape = new btBoxShape(btVector3(1.0f, 1.0f, 1.0f));

    btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
        btQuaternion(0, 0, 0, 1),
        btVector3(0, 0, 0)
    ));

    btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
        1,                  // mass, in kg. 0 -> Static object, will never move.
        motionstate,
        boxCollisionShape,  // collision shape of body
        btVector3(0,0,0)    // local inertia
    );
    btRigidBody *rigidBody = new btRigidBody(rigidBodyCI);

    rigidBody->setLinearVelocity(btVector3(1.0f, 0.0f, 0.0f));

    dynamicsWorld->addRigidBody(rigidBody);

    while (1 == 1)
    {
        dynamicsWorld->stepSimulation(1.0f);
        btTransform trans = rigidBody->getWorldTransform();
        btVector3 origin = trans.getOrigin();
        std::cout << origin.getX() << ":" << origin.getY() << ":" << origin.getZ() << std::endl;
        sleep(1);
    }

    return 0;
}