#define FREEGLUT_STATIC
#include <GL/freeglut.h>
#include <GL/glu.h>
#include <iostream>
#include <btBulletDynamicsCommon.h>
 
GLfloat floorMaterialDiffuse[] = { 0.55f, 0.64f, 0.36f, 1.f };
GLfloat sphereMaterialDiffuse[] = { 0.06f, 0.29f, 0.65f, 1.f };
GLfloat platformMaterialDiffuse[] = { 0.74f, 0.53f, 0.19f, 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 sphereRadius = 0.5f;
float sphereMass = 1.f;
float spherePosX = -2.f;
float spherePosY = 7.f;
float spherePosZ = 0.f;
btRigidBody *sphereRigidBody;
btDefaultMotionState *sphereMotionState;
btSphereShape *sphereShape;

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

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

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

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));

    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(0.2f);
    // sphereRigidBody->setRollingFriction(0.5f);
    sphereRigidBody->setRestitution(1.f);
    world->addRigidBody(sphereRigidBody);
    
    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(0.7f);
    floorRigidBody->setFriction(0.2f);
    world->addRigidBody(floorRigidBody);

    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.924f, 0.383f, 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(0.7f);
    firstPlatformRigidBody->setFriction(0.2f);
    world->addRigidBody(firstPlatformRigidBody);

    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.924f, -0.383f, 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(0.9f);
    secondPlatformRigidBody->setFriction(0.5f);
    world->addRigidBody(secondPlatformRigidBody);
}

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;
}

void draw()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    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();
    
    // 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();
    
    glutSwapBuffers();
}
 
void timer(int)
{
    world->stepSimulation(0.016f, 8);
    glutPostRedisplay();
    glutTimerFunc(16, timer, 0);
}
 
void resize(int w, int h)
{
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(50.f, (GLfloat) w / (GLfloat) h, 0.1f, 100.f);
}
 
int main(int argc, char *argv[])
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
    int winW = 400, winH = 400;
    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);
    glutDisplayFunc(draw);
    initPhysics();
    timer(0);
    glutReshapeFunc(resize);
    glutMainLoop();

    cleanUp();
    return 0;
}
