#include "OdeWorld.h"
#include <vector>
#include <fstream>
#include <iostream>
#include <GL/gl.h>
using namespace std;

#define WORLD_STEP 0.001
#define WHITESPACE " \t\r\n\f"

static string trim(const string & orig)
{
    string result = orig;
    size_t pos = result.find_first_not_of(WHITESPACE);
    if (pos == string::npos)
    {
        result = "";
    }
    else
    {
        if (pos > 0)
            result = result.substr(pos, result.length() - pos);
        pos = result.find_last_not_of(WHITESPACE);
        if (pos < result.length() - 1)
            result = result.substr(0, pos + 1);
    }
    return result;
}

/* used by ODE to perform collision detection */
void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2)
{
    const int maxNumContacts = 4;
    OdeWorld * ow = (OdeWorld *) data;
    static dContact contact[maxNumContacts];
    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);
    if ((b1 == b2) || (b1 && b2 && dAreConnected(b1, b2)))
        return;
    int num = dCollide(o1, o2, maxNumContacts,
            &contact[0].geom, sizeof(contact[0]));
    for (int i = 0; i < num; i++)
    {
        contact[i].surface.mode =
            dContactSlip1 | dContactSlip2 | dContactBounce |
            dContactSoftERP | dContactSoftCFM | dContactApprox1;
        contact[i].surface.mu = 0.5;
        contact[i].surface.slip1 = 0.0;
        contact[i].surface.slip2 = 0.0;
        contact[i].surface.soft_erp = 0.8;
        contact[i].surface.soft_cfm = 0.01;
        contact[i].surface.bounce = 0.0;
        dJointID joint = dJointCreateContact(ow->m_world,
                ow->m_contactJointGroup, contact + i);
        dJointAttach(joint, b1, b2);
    }
}

OdeWorld::OdeWorld()
{
    m_world = dWorldCreate();
    m_space = dHashSpaceCreate(0);
    m_contactJointGroup = dJointGroupCreate(0);
    setGravity(0, 0, -9.81);
}

OdeWorld::~OdeWorld()
{
    dJointGroupDestroy(m_contactJointGroup);
    dSpaceDestroy(m_space);
    dWorldDestroy(m_world);
}

/* invokes ODE to do physics on our world */
void OdeWorld::step()
{
    dSpaceCollide(m_space, this, OdeWorld_collide_callback);
    dWorldQuickStep(m_world, WORLD_STEP);
    dJointGroupEmpty(m_contactJointGroup);
}

OdeWorld::Object * OdeWorld::createObject(bool is_static)
{
    return new Object(is_static, m_world, m_space);
}

/* push an OpenGL matrix onto the matrix stack for a given
 * ODE body position and rotation */
void OdeWorld::pushTransform(const dReal * pos, const dReal * R)
{
    GLfloat matrix[16];
    matrix[0] = R[0];
    matrix[1] = R[4];
    matrix[2] = R[8];
    matrix[3] = 0;
    matrix[4] = R[1];
    matrix[5] = R[5];
    matrix[6] = R[9];
    matrix[7] = 0;
    matrix[8] = R[2];
    matrix[9] = R[6];
    matrix[10] = R[10];
    matrix[11] = 0;
    matrix[12] = pos[0];
    matrix[13] = pos[1];
    matrix[14] = pos[2];
    matrix[15] = 1;
    glPushMatrix();
    glMultMatrixf(matrix);
}

OdeWorld::Object::Object(bool is_static, dWorldID world, dSpaceID space)
{
    m_is_static = is_static;
    m_world = world;
    m_space = space;
    m_body = 0;
}

void OdeWorld::Object::setPosition(double x, double y, double z)
{
    if (m_is_static)
    {
        for (int i = 0, sz = m_geoms.size(); i < sz; i++)
            dGeomSetPosition(m_geoms[i], x, y, z);
    }
    else
    {
        if (m_body != 0)
            dBodySetPosition(m_body, x, y, z);
    }
}

void OdeWorld::Object::getPosition(double * x, double * y, double * z)
{
    const dReal * pos = NULL;
    if (m_is_static)
    {
        if (m_geoms.size() > 0)
            pos = dGeomGetPosition(m_geoms[0]);
    }
    else
    {
        if (m_body != 0)
            pos = dBodyGetPosition(m_body);
    }

    if (pos != NULL)
    {
        *x = pos[0];
        *y = pos[1];
        *z = pos[2];
    }
}

void OdeWorld::Object::loadPhy(const std::string & path)
{
    ifstream ifs(path.c_str());
    if (ifs.is_open())
    {
        while (!ifs.eof())
        {
            string line;
            getline(ifs, line);
            line = trim(line);
            if (line == "" || line[0] == '#')
                continue;
            size_t pos = line.find_first_of(WHITESPACE);
            if (pos == string::npos)
                continue;
            string type = line.substr(0, pos);
            pos = line.find("\"", pos);
            if (pos == string::npos)
                continue;
            size_t pos2 = line.find("\"", pos + 1);
            if (pos2 == string::npos)
                continue;
            string name = line.substr(pos + 1, pos2 - pos - 1);
            pos = pos2 + 1;
            vector<float> args;
            for (;;)
            {
                pos = line.find_first_not_of(WHITESPACE, pos);
                if (pos == string::npos)
                    break;
                pos2 = line.find_first_of(WHITESPACE, pos);
                string n = line.substr(pos, pos2 - pos);
                float f = atof(n.c_str());
                args.push_back(f);
                if (pos2 == string::npos)
                    break;
                pos = pos2 + 1;
            }
            if      (type == "cube")
            {
                addCube(args);
            }
            else if (type == "sphere")
            {
                addSphere(args);
            }
            else if (type == "cylinder")
            {
                addCylinder(args);
            }
            else if (type == "plane")
            {
                addPlane(args);
            }
        }
    }
}

void OdeWorld::Object::addCube(const vector<float> args)
{
    if (args.size() != 9)
        return;
    dGeomID id = dCreateBox(0, args[0], args[1], args[2]);
    dMass mass;
    dMassSetBox(&mass, 1.0, args[0], args[1], args[2]);
    setupGeom(id, &mass,
            args[3], args[4], args[5],
            args[6], args[7], args[8]);
}

void OdeWorld::Object::addSphere(const vector<float> args)
{
    if (args.size() != 4)
        return;
    dGeomID id = dCreateSphere(0, args[0]);
    dMass mass;
    dMassSetSphere(&mass, 1.0, args[0]);
    setupGeom(id, &mass,
            args[1], args[2], args[3],
            0.0, 0.0, 0.0);
}

void OdeWorld::Object::addCylinder(const vector<float> args)
{
    if (args.size() != 8)
        return;
    dGeomID id = dCreateCylinder(0, args[0], args[1]);
    dMass mass;
    dMassSetCylinder(&mass, 1.0, 3, args[0], args[1]);
    setupGeom(id, &mass,
            args[2], args[3], args[4],
            args[5], args[6], args[7]);
}

void OdeWorld::Object::addCCylinder(const vector<float> args)
{
    if (args.size() != 8)
        return;
    dGeomID id = dCreateCCylinder(0, args[0], args[1]);
    dMass mass;
    dMassSetCappedCylinder(&mass, 1.0, 3, args[0], args[1]);
    setupGeom(id, &mass,
            args[2], args[3], args[4],
            args[5], args[6], args[7]);
}

void OdeWorld::Object::addPlane(const vector<float> args)
{
    if (args.size() != 6)
        return;

    dMatrix3 r;
    dRFromEulerAngles(r, args[3], args[4], args[5]);
    dVector3 default_plane_direction = {0, 0, 1, 0};
    dVector3 rotated_plane_direction;
    dMultiply0(rotated_plane_direction, default_plane_direction, r, 1, 3, 3);

    float a = rotated_plane_direction[0];
    float b = rotated_plane_direction[1];
    float c = rotated_plane_direction[2];
    float d = a * args[0] + b * args[1] + c * args[2];

    dGeomID id = dCreatePlane(m_space, a, b, c, d);
    m_geoms.push_back(id);
}

void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass,
        float locx, float locy, float locz,
        float rotx, float roty, float rotz)
{
    dMatrix3 rot;
    dRFromEulerAngles(rot, rotx, roty, rotz);
    dGeomSetRotation(geom, rot);
    dGeomSetPosition(geom, locx, locy, locz);

    dGeomID transform = dCreateGeomTransform(m_space);
    dGeomTransformSetCleanup(transform, 1);
    dGeomTransformSetInfo(transform, 1);
    dGeomTransformSetGeom(transform, geom);

    m_geoms.push_back(transform);

    if (!m_is_static)
    {
        /* attach the geometry to the body */
        if (m_body == 0)
            m_body = dBodyCreate(m_world);

        dGeomSetBody(transform, m_body);

        dMassRotate(mass, rot);
        dMassTranslate(mass, locx, locy, locz);
        dMass origmass;
        dBodyGetMass(m_body, &origmass);
        dMassAdd(&origmass, mass);
        dBodySetMass(m_body, &origmass);
    }
}

const dReal * OdeWorld::Object::getPosition()
{
    if (m_body != 0)
    {
        return dBodyGetPosition(m_body);
    }
    else if (m_geoms.size() > 0)
    {
        return dGeomGetPosition(m_geoms[0]);
    }
    return NULL;
}

const dReal * OdeWorld::Object::getRotation()
{
    if (m_body != 0)
    {
        return dBodyGetRotation(m_body);
    }
    else if (m_geoms.size() > 0)
    {
        return dGeomGetRotation(m_geoms[0]);
    }
    return NULL;
}