#define FREEGLUT_STATIC
#include <GL/freeglut.h>
#include <GL/glu.h>
#include <algorithm>
#include <btBulletDynamicsCommon.h>
#include <iostream>
#include <set>

#define EXPLOSION_STRENGTH 50.f

int winW = 350, winH = 350;

// Convenient typedefs for collision events
typedef std::pair<const btRigidBody *, const btRigidBody *> CollisionPair;
typedef std::set<CollisionPair> CollisionPairs;

// Collision event variables
CollisionPairs pairsLastUpdate;

std::string outputText1 = "Press the C button to enable/disable";
std::string outputText2 = "the drawing of colliders.";

GLfloat floorMaterialDiffuse[] = { 0.55f, 0.64f, 0.36f, 1.f };
GLfloat sphereMaterialDiffuse[] = { 0.06f, 0.29f, 0.65f, 1.f };
GLfloat boxMaterialDiffuse[] = { 0.16f, 0.65f, 0.29f, 1.f };
GLfloat platformMaterialDiffuse[] = { 0.74f, 0.53f, 0.19f, 1.f };
GLfloat colliderMaterialDiffuse[] = { 0.f, 1.f, 0.f, 1.f };
GLfloat textMaterialDiffuse[] = { 1.f, 1.f, 1.f, 1.f };
GLfloat light0Diffuse[] = { 1.f, 1.f, 1.0f };
GLfloat light0Direction[] = { 2.f, 5.f, 4.f, 1.f };

btDynamicsWorld *world;
btCollisionConfiguration *collisionConfiguration;
btConstraintSolver *solver;
btBroadphaseInterface *overlappingPairCache;
btCollisionDispatcher *dispatcher;

float spherePosX = -2.f;
float spherePosY = 5.f;
float spherePosZ = 0.f;
float sphereRadius = 0.5f;
float sphereMass = 1.f;
btRigidBody *sphereRigidBody;
btDefaultMotionState *sphereMotionState;
btSphereShape *sphereShape;

float boxPosX = 0.f;
float boxPosY = 2.f;
float boxPosZ = -3.f;
float boxSize = 1.f;
float boxMass = 1.f;
btRigidBody *boxRigidBody;
btDefaultMotionState *boxMotionState;
btBoxShape *boxShape;

float floorPosX = 0.f;
float floorPosY = 0.f;
float floorPosZ = 0.f;
float floorWidth = 10.f;
float floorHeight = 0.2f;
float floorDepth = 10.f;
btRigidBody *floorRigidBody;
btDefaultMotionState *floorMotionState;
btBoxShape *floorShape;

float firstPlatformPosX = 3.f;
float firstPlatformPosY = 3.f;
float firstPlatformPosZ = 0.f;
float firstPlatformWidth = 3.f;
float firstPlatformHeight = 0.2f;
float firstPlatformDepth = 0.5f;
btRigidBody *firstPlatformRigidBody;
btDefaultMotionState *firstPlatformMotionState;
btBoxShape *firstPlatformShape;

float secondPlatformPosX = -1.f;
float secondPlatformPosY = 4.f;
float secondPlatformPosZ = 0.f;
float secondPlatformWidth = 3.f;
float secondPlatformHeight = 0.2f;
float secondPlatformDepth = 0.5f;
btRigidBody *secondPlatformRigidBody;
btDefaultMotionState *secondPlatformMotionState;
btBoxShape *secondPlatformShape;

// btCollisionObject *trigger;
btCollisionObject *explosion;

// Collision groups for different types of objects. Each value
// is represented by a single bit
enum CollisionGroups
{
    COLGROUP_NONE = 0,
    COLGROUP_STATIC = 1 << 0,
    COLGROUP_EXPLOSION = 1 << 1,
    COLGROUP_SPHERE = 1 << 2,
    COLGROUP_DYNAMIC = 1 << 3
};

bool drawColliders = true;

class DebugDrawer : public btIDebugDraw
{
public:
    // Debug mode functions
    virtual void setDebugMode(int debugMode) override { m_debugMode = debugMode; }
    virtual int getDebugMode() const override { return m_debugMode; }

    // Draws a line between two contact points
    virtual void drawContactPoint(const btVector3 &pointOnB, const btVector3 &normalOnB,
        btScalar distance, int lifeTime, const btVector3 &color) override
    {
        btVector3 const startPoint = pointOnB;
        btVector3 const endPoint = pointOnB + normalOnB * distance;
        drawLine(startPoint, endPoint, color);
    }

    // Draws a simple line of pixels between points
    virtual void drawLine(const btVector3 &from, const btVector3 &to,
        const btVector3 &color) override
    {

        // Use the GL_LINES primitive to draw lines
        glBegin(GL_LINES);
        // glColor3f(color.getX(), color.getY(), color.getZ());
        glMaterialfv(GL_FRONT, GL_DIFFUSE, colliderMaterialDiffuse);
        glVertex3f(from.getX(), from.getY(), from.getZ());
        glVertex3f(to.getX(), to.getY(), to.getZ());
        glEnd();
    }

    // Unused
    virtual void reportErrorWarning(const char *warningString) override { }
    virtual void draw3dText(const btVector3 &location, const char *textString) override { }

    void toggleDebugFlag(int flag)
    {
        // Checks if a flag is set and enables/disables it
        if (m_debugMode & flag)
        {
            // Flag is enabled, so disable it
            m_debugMode = m_debugMode & (~flag);
        }
        else
        {
            // Flag is disabled, so enable it
            m_debugMode |= flag;
        }
    }

protected:
    int m_debugMode;
};

DebugDrawer *debugDrawer;

void initPhysics()
{
    collisionConfiguration = new btDefaultCollisionConfiguration();
    dispatcher = new btCollisionDispatcher(collisionConfiguration);
    overlappingPairCache = new btDbvtBroadphase();
    solver = new btSequentialImpulseConstraintSolver();
    world = new btDiscreteDynamicsWorld(dispatcher, overlappingPairCache,
        solver, collisionConfiguration);
    // std::cout << world->getGravity().y() << std::endl;
    world->setGravity(btVector3(0.f, -9.8f, 0.f));

    debugDrawer = new DebugDrawer();
    debugDrawer->setDebugMode(0); // Set the initial debug level to 0
    debugDrawer->toggleDebugFlag(btIDebugDraw::DBG_MAX_DEBUG_DRAW_MODE);
    world->setDebugDrawer(debugDrawer);

    float restitution = 0.7f;
    float friction = 0.9f;

    sphereShape = new btSphereShape(sphereRadius);
    btTransform sphereTransform;
    sphereTransform.setIdentity();
    sphereTransform.setOrigin(btVector3(spherePosX, spherePosY, spherePosZ));
    sphereTransform.setRotation(btQuaternion(1.f, 0.f, 0.f, 0.f));
    btVector3 sphereLocalInertia(0.f, 0.f, 0.f);
    sphereShape->calculateLocalInertia(sphereMass, sphereLocalInertia);
    sphereMotionState = new btDefaultMotionState(sphereTransform);
    btRigidBody::btRigidBodyConstructionInfo sphereRbInfo(sphereMass, sphereMotionState,
        sphereShape, sphereLocalInertia);
    sphereRigidBody = new btRigidBody(sphereRbInfo);
    sphereRigidBody->setActivationState(DISABLE_DEACTIVATION);
    sphereRigidBody->setFriction(friction);
    // sphereRigidBody->setRollingFriction(0.5f);
    sphereRigidBody->setRestitution(restitution);
    // sphereRigidBody->setCustomDebugColor(btVector3(1.f, 0.f, 0.f));
    world->addRigidBody(sphereRigidBody, COLGROUP_SPHERE, COLGROUP_STATIC);

    boxShape = new btBoxShape(btVector3(boxSize / 2.f, boxSize / 2.f,
        boxSize / 2.f));
    btTransform boxTransform;
    boxTransform.setIdentity();
    boxTransform.setOrigin(btVector3(boxPosX, boxPosY, boxPosZ));
    boxTransform.setRotation(btQuaternion(1.f, 0.f, 0.f, 0.f));
    btVector3 boxLocalInertia(0.f, 0.f, 0.f);
    boxShape->calculateLocalInertia(boxMass, boxLocalInertia);
    boxMotionState = new btDefaultMotionState(boxTransform);
    btRigidBody::btRigidBodyConstructionInfo boxRbInfo(boxMass, boxMotionState,
        boxShape, boxLocalInertia);
    boxRigidBody = new btRigidBody(boxRbInfo);
    boxRigidBody->setActivationState(DISABLE_DEACTIVATION);
    boxRigidBody->setFriction(friction);
    // boxRigidBody->setRollingFriction(0.5f);
    boxRigidBody->setRestitution(restitution);
    // boxRigidBody->setCustomDebugColor(btVector3(1.f, 0.f, 0.f));
    world->addRigidBody(boxRigidBody, COLGROUP_DYNAMIC, COLGROUP_STATIC);

    floorShape = new btBoxShape(btVector3(floorWidth / 2.f, floorHeight / 2.f,
        floorDepth / 2.f));
    btTransform floorTransform;
    floorTransform.setIdentity();
    floorTransform.setOrigin(btVector3(floorPosX, floorPosY, floorPosZ));
    floorTransform.setRotation(btQuaternion(1.f, 0.f, 0.f, 0.f));
    btVector3 floorLocalInertia(0.f, 0.f, 0.f);
    float floorMass = 0.f;
    floorMotionState = new btDefaultMotionState(floorTransform);
    btRigidBody::btRigidBodyConstructionInfo floorRbInfo(floorMass, floorMotionState,
        floorShape, floorLocalInertia);
    floorRigidBody = new btRigidBody(floorRbInfo);
    floorRigidBody->setActivationState(DISABLE_DEACTIVATION);
    // std::cout << floorRigidBody->getRestitution() << std::endl;
    floorRigidBody->setRestitution(restitution);
    floorRigidBody->setFriction(friction);
    world->addRigidBody(floorRigidBody, COLGROUP_STATIC, COLGROUP_SPHERE | COLGROUP_DYNAMIC);

    firstPlatformShape = new btBoxShape(btVector3(firstPlatformWidth / 2.f,
        firstPlatformHeight / 2.f, firstPlatformDepth / 2.f));
    btTransform firstPlatformTransform;
    firstPlatformTransform.setIdentity();
    firstPlatformTransform.setOrigin(btVector3(firstPlatformPosX, firstPlatformPosY,
        firstPlatformPosZ));
    firstPlatformTransform.setRotation(btQuaternion(0.985f, 0.174, 0.f, 0.f));
    btVector3 firstPlatformLocalInertia(0.f, 0.f, 0.f);
    float firstPlatformMass = 0.f;
    firstPlatformMotionState = new btDefaultMotionState(firstPlatformTransform);
    btRigidBody::btRigidBodyConstructionInfo firstPlatformRbInfo(firstPlatformMass,
        firstPlatformMotionState, firstPlatformShape, firstPlatformLocalInertia);
    firstPlatformRigidBody = new btRigidBody(firstPlatformRbInfo);
    firstPlatformRigidBody->setActivationState(DISABLE_DEACTIVATION);
    firstPlatformRigidBody->setRestitution(restitution);
    firstPlatformRigidBody->setFriction(friction);
    world->addRigidBody(firstPlatformRigidBody, COLGROUP_STATIC, COLGROUP_SPHERE);

    secondPlatformShape = new btBoxShape(btVector3(secondPlatformWidth / 2.f,
        secondPlatformHeight / 2.f, secondPlatformDepth / 2.f));
    btTransform secondPlatformTransform;
    secondPlatformTransform.setIdentity();
    secondPlatformTransform.setOrigin(btVector3(secondPlatformPosX, secondPlatformPosY,
        secondPlatformPosZ));
    secondPlatformTransform.setRotation(btQuaternion(0.985f, -0.174, 0.f, 0.f));
    btVector3 secondPlatformLocalInertia(0.f, 0.f, 0.f);
    float secondPlatformMass = 0.f;
    secondPlatformMotionState = new btDefaultMotionState(secondPlatformTransform);
    btRigidBody::btRigidBodyConstructionInfo secondPlatformRbInfo(firstPlatformMass,
        secondPlatformMotionState, secondPlatformShape, secondPlatformLocalInertia);
    secondPlatformRigidBody = new btRigidBody(secondPlatformRbInfo);
    secondPlatformRigidBody->setActivationState(DISABLE_DEACTIVATION);
    secondPlatformRigidBody->setRestitution(restitution);
    secondPlatformRigidBody->setFriction(friction);
    world->addRigidBody(secondPlatformRigidBody, COLGROUP_STATIC, COLGROUP_SPHERE);

    // trigger = new btCollisionObject();
    // trigger->setCollisionShape(new btBoxShape(btVector3(1.f, 0.25f, 1.f)));
    // btTransform triggerTrans;
    // triggerTrans.setIdentity();
    // triggerTrans.setOrigin(btVector3(0.f, 2.f, 0.f));
    // trigger->setWorldTransform(triggerTrans);
    // trigger->setCollisionFlags(btCollisionObject::CF_NO_CONTACT_RESPONSE);
    // world->addCollisionObject(trigger);

    explosion = new btCollisionObject();
    explosion->setCollisionShape(new btSphereShape(1.f));
    btTransform explodeTrans;
    explodeTrans.setIdentity();
    explodeTrans.setOrigin(btVector3(0.f, 0.f, 0.f));
    explosion->setWorldTransform(explodeTrans);
    explosion->setCollisionFlags(btCollisionObject::CF_NO_CONTACT_RESPONSE);
    world->addCollisionObject(explosion);
}

void cleanUp()
{
    delete world;
    delete solver;
    delete overlappingPairCache;
    delete dispatcher;
    delete collisionConfiguration;
    delete sphereRigidBody;
    delete sphereMotionState;
    delete sphereShape;
    delete floorRigidBody;
    delete floorMotionState;
    delete floorShape;
    delete firstPlatformRigidBody;
    delete firstPlatformMotionState;
    delete firstPlatformShape;
    delete secondPlatformRigidBody;
    delete secondPlatformMotionState;
    delete secondPlatformShape;
    delete debugDrawer;
    // delete trigger;
    if (explosion)
    {
        delete explosion;
    }
    delete boxRigidBody;
    delete boxMotionState;
    delete boxShape;
}

void resize(int w, int h)
{
    winW = w;
    winH = h;
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(50.f, (GLfloat)w / (GLfloat)h, 0.1f, 100.f);
}

/*
GLUT_BITMAP_8_BY_13
GLUT_BITMAP_9_BY_15
GLUT_BITMAP_TIMES_ROMAN_10
GLUT_BITMAP_TIMES_ROMAN_24
GLUT_BITMAP_HELVETICA_10
GLUT_BITMAP_HELVETICA_12
GLUT_BITMAP_HELVETICA_18
*/
void drawText(float x, float y, std::string text)
{
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, 100, 100, 0, -100, 100);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    gluLookAt(
        0.f, 0.f, 10.f,
        0.f, 0.f, 0.f,
        0.f, 1.f, 0.f);

    glMaterialfv(GL_FRONT, GL_DIFFUSE, textMaterialDiffuse);
    glScalef(2.f, 2.f, 2.f);
    glRasterPos2f(x, y);
    glutBitmapString(GLUT_BITMAP_9_BY_15, (const unsigned char *)text.c_str());
}

void draw()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glDisable(GL_LIGHTING);
    drawText(3, 5, outputText1);
    drawText(3, 7, outputText2);
    glEnable(GL_LIGHTING);

    resize(winW, winH);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    gluLookAt(
        -3.f, 6.f, 7.f,
        0.f, 3.f, 0.f,
        0.f, 1.f, 0.f);

    glPushMatrix();
    {
        glMaterialfv(GL_FRONT, GL_DIFFUSE, sphereMaterialDiffuse);
        btScalar transform[16];
        btMotionState *motionState = sphereRigidBody->getMotionState();
        btTransform tempTransform;
        motionState->getWorldTransform(tempTransform);
        tempTransform.getOpenGLMatrix(transform);
        glMultMatrixf(transform);
        glutSolidSphere(sphereRadius, 32, 16);
    }
    glPopMatrix();

    glPushMatrix();
    {
        glMaterialfv(GL_FRONT, GL_DIFFUSE, boxMaterialDiffuse);
        btScalar transform[16];
        btMotionState *motionState = boxRigidBody->getMotionState();
        btTransform tempTransform;
        motionState->getWorldTransform(tempTransform);
        tempTransform.getOpenGLMatrix(transform);
        glMultMatrixf(transform);
        glScalef(boxSize, boxSize, boxSize);
        glutSolidCube(1.f);
    }
    glPopMatrix();

    // Draw the floor
    glPushMatrix();
    {
        glMaterialfv(GL_FRONT, GL_DIFFUSE, floorMaterialDiffuse);
        btScalar transform[16];
        btMotionState *motionState = floorRigidBody->getMotionState();
        btTransform tempTransform;
        motionState->getWorldTransform(tempTransform);
        tempTransform.getOpenGLMatrix(transform);
        glMultMatrixf(transform);
        glScalef(floorWidth, floorHeight, floorDepth);
        glutSolidCube(1.f);
    }
    glPopMatrix();

    // Draw the first platform
    glPushMatrix();
    {
        glMaterialfv(GL_FRONT, GL_DIFFUSE, platformMaterialDiffuse);
        btScalar transform[16];
        btMotionState *motionState = firstPlatformRigidBody->getMotionState();
        btTransform tempTransform;
        motionState->getWorldTransform(tempTransform);
        tempTransform.getOpenGLMatrix(transform);
        glMultMatrixf(transform);
        glScalef(firstPlatformWidth, firstPlatformHeight, firstPlatformDepth);
        glutSolidCube(1.f);
    }
    glPopMatrix();

    // Draw the second platform
    glPushMatrix();
    {
        glMaterialfv(GL_FRONT, GL_DIFFUSE, platformMaterialDiffuse);
        btScalar transform[16];
        btMotionState *motionState = secondPlatformRigidBody->getMotionState();
        btTransform tempTransform;
        motionState->getWorldTransform(tempTransform);
        tempTransform.getOpenGLMatrix(transform);
        glMultMatrixf(transform);
        glScalef(secondPlatformWidth, secondPlatformHeight, secondPlatformDepth);
        glutSolidCube(1.f);
    }
    glPopMatrix();

    world->debugDrawWorld();

    glutSwapBuffers();
}

void collisionEvent(btRigidBody *pBody0, btRigidBody *pBody1)
{
    // Did the box collide with the trigger?
    // if (pBody0 == sphereRigidBody && pBody1 == trigger ||
    // pBody1 == sphereRigidBody && pBody0 == trigger)
    // {
    // std::cout << "Collision with trigger" << std::endl;
    // outputText = "Collision with trigger";
    // }

    if (pBody0 == explosion || pBody1 == explosion)
    {
        // std::cout << "Collision with explosion" << std::endl;
        // outputText = "Collision with explosion";

        // Get the pointer of the other object
        btRigidBody *pOther;
        if (pBody0 == explosion)
        {
            pOther = (btRigidBody *)pBody1;
        }
        else
        {
            pOther = (btRigidBody *)pBody0;
        }

        // Wake the object up
        pOther->setActivationState(ACTIVE_TAG);

        // Calculate the vector between the object and
        // the center of the explosion
        btVector3 dir = pOther->getWorldTransform().getOrigin() -
                        explosion->getWorldTransform().getOrigin();
        // Get the distance
        float dist = dir.length();
        // Calculate the impulse strength
        float strength = EXPLOSION_STRENGTH;
        // Follow an inverse-distance rule
        if (dist != 0.f)
        {
            strength /= dist;
        }
        // Normalize the direction vector
        dir.normalize();
        // Apply the impulse
        pOther->applyCentralImpulse(dir * strength);

        // Box
        btVector3 boxDir = boxRigidBody->getWorldTransform().getOrigin() -
                           explosion->getWorldTransform().getOrigin();
        // Get the distance
        float boxDist = boxDir.length();
        // Calculate the impulse strength
        // Follow an inverse-distance rule
        if (boxDist != 0.f)
        {
            strength /= boxDist;
        }
        // Normalize the direction vector
        boxDir.normalize();
        // Apply the impulse
        boxRigidBody->applyCentralImpulse(boxDir * strength);

        // Destroy the explosion object
        if (explosion)
        {
            world->removeCollisionObject(explosion);
            delete explosion;
            explosion = 0;
        }
    }
}

void separationEvent(btRigidBody *pBody0, btRigidBody *pBody1)
{
    // std::cout << "Separation" << std::endl;
    // outputText = "Separation";
}

void checkForCollisionEvents()
{
    // Keep a list of the collision pairs we
    // found during the current update
    CollisionPairs pairsThisUpdate;

    // Iterate through all of the manifolds in the dispatcher
    for (int i = 0; i < dispatcher->getNumManifolds(); ++i)
    {
        // Get the manifold
        btPersistentManifold *pManifold = dispatcher->getManifoldByIndexInternal(i);

        // Ignore manifolds that have no contact points
        if (pManifold->getNumContacts() > 0)
        {
            // Get the two rigid bodies involved in the collision
            const btRigidBody *pBody0 = static_cast<const btRigidBody *>(pManifold->getBody0());
            const btRigidBody *pBody1 = static_cast<const btRigidBody *>(pManifold->getBody1());

            // Always create the pair in a predictable order (use the pointer value..)
            bool const swapped = pBody0 > pBody1;
            const btRigidBody *pSortedBodyA = swapped ? pBody1 : pBody0;
            const btRigidBody *pSortedBodyB = swapped ? pBody0 : pBody1;

            // Create the pair
            CollisionPair thisPair = std::make_pair(pSortedBodyA, pSortedBodyB);

            // Insert the pair into the current list
            pairsThisUpdate.insert(thisPair);

            // If this pair doesn't exist in the list
            // from the previous update, it is a new
            // pair and we must send a collision event
            if (pairsLastUpdate.find(thisPair) == pairsLastUpdate.end())
            {
                collisionEvent((btRigidBody *)pBody0, (btRigidBody *)pBody1);
            }
        }
    }

    // Create another list for pairs that were removed this update
    CollisionPairs removedPairs;

    // This handy function gets the difference between
    // two sets. It takes the difference between
    // collision pairs from the last update, and this
    // update and pushes them into the removed pairs list
    std::set_difference(pairsLastUpdate.begin(), pairsLastUpdate.end(),
        pairsThisUpdate.begin(), pairsThisUpdate.end(),
        std::inserter(removedPairs, removedPairs.begin()));

    // Iterate through all of the removed pairs sending separation events for them
    for (CollisionPairs::const_iterator iter = removedPairs.begin();
         iter != removedPairs.end(); ++iter)
    {
        separationEvent((btRigidBody *)iter->first, (btRigidBody *)iter->second);
    }

    // In the next iteration we'll want to compare against the pairs we found
    // in this iteration
    pairsLastUpdate = pairsThisUpdate;
}

void timer(int)
{
    world->stepSimulation(0.016, 8);
    checkForCollisionEvents();

    // Destroy the explosion object after one iteration
    // if (explosion)
    // {
    // world->removeCollisionObject(explosion);
    // delete explosion;
    // explosion = 0;
    // }

    glutPostRedisplay();
    glutTimerFunc(16.f, timer, 0);
}

void keyboardCallback(unsigned char key, int x, int y)
{
    switch (key)
    {
    case 'c': {
        if (drawColliders)
        {
            drawColliders = false;
            debugDrawer->setDebugMode(btIDebugDraw::DBG_NoDebug);
        }
        else
        {
            debugDrawer->setDebugMode(btIDebugDraw::DBG_MAX_DEBUG_DRAW_MODE);
            drawColliders = true;
        }
        break;
    }
    }
}

int main(int argc, char *argv[])
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
    glutInitWindowSize(winW, winH);
    glutInitWindowPosition(100, 100);
    int window = glutCreateWindow("OpenGL1, FreeGLUT, C++");
    glClearColor(0.2f, 0.2f, 0.2f, 1.f);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, light0Diffuse);
    glLightfv(GL_LIGHT0, GL_POSITION, light0Direction);
    glEnable(GL_NORMALIZE);
    glutKeyboardFunc(keyboardCallback);
    glutDisplayFunc(draw);
    initPhysics();
    timer(0);
    glutReshapeFunc(resize);
    glutMainLoop();

    cleanUp();
    return 0;
}
