#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.1;
        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, float scale)
{
    return new Object(is_static, m_world, m_space, scale);
}

/* 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,
        float scale)
{
    m_is_static = is_static;
    m_world = world;
    m_space = space;
    m_body = 0;
    m_scale = scale;
    dMassSetZero(&m_mass);
    for (int i = 0; i < 3; i++)
        m_position[i] = 0.0;
    for (int j = 0; j < 3; j++)
        for (int i = 0; i < 4; i++)
            m_rotation[j * 4 + i] = (j == i) ? 1.0 : 0.0;
}

OdeWorld::Object::Object(const OdeWorld::Object & orig)
{
    m_is_static = orig.m_is_static;
    m_world = orig.m_world;
    m_space = orig.m_space;
    m_scale = orig.m_scale;
    for (int i = 0; i < 3; i++)
        m_position[i] = orig.m_position[i];
    for (int i = 0; i < 12; i++)
        m_rotation[i] = orig.m_rotation[i];
    /* make a copy of the ODE body */
    if (orig.m_body != 0)
    {
        m_body = dBodyCreate(m_world);
        m_mass = orig.m_mass;
        dBodySetMass(m_body, &m_mass);
        const dReal * pos = dBodyGetPosition(orig.m_body);
        dBodySetPosition(m_body, pos[0], pos[1], pos[2]);
        const dReal * rot = dBodyGetRotation(orig.m_body);
        dBodySetRotation(m_body, rot);
        dBodySetAutoDisableFlag(m_body,
                dBodyGetAutoDisableFlag(orig.m_body));
        dBodySetAutoDisableLinearThreshold(m_body,
                dBodyGetAutoDisableLinearThreshold(orig.m_body));
        dBodySetAutoDisableAngularThreshold(m_body,
                dBodyGetAutoDisableAngularThreshold(orig.m_body));
        dBodySetAutoDisableSteps(m_body,
                dBodyGetAutoDisableSteps(orig.m_body));
        dBodySetAutoDisableTime(m_body,
                dBodyGetAutoDisableTime(orig.m_body));
        int finite_rotation_mode = dBodyGetFiniteRotationMode(orig.m_body);
        dBodySetFiniteRotationMode(m_body, finite_rotation_mode);
        if (finite_rotation_mode)
        {
            dVector3 finite_rotation_axis;
            dBodyGetFiniteRotationAxis(orig.m_body, finite_rotation_axis);
            dBodySetFiniteRotationAxis(m_body,
                    finite_rotation_axis[0],
                    finite_rotation_axis[1],
                    finite_rotation_axis[2]);
        }
        dBodySetGravityMode(m_body, dBodyGetGravityMode(orig.m_body));
    }
    else
    {
        m_body = 0;
    }
    /* make a copy of the ODE geoms */
    for (int i = 0, sz = orig.m_geoms.size(); i < sz; i++)
    {
        m_geoms.push_back(cloneGeom(orig.m_geoms[i], m_body));
    }
}

OdeWorld::Object::~Object()
{
    for (int i = 0, sz = m_geoms.size(); i < sz; i++)
        dGeomDestroy(m_geoms[i]);
    if (m_body != 0)
        dBodyDestroy(m_body);
}

dGeomID OdeWorld::Object::cloneGeom(dGeomID geom, dBodyID body)
{
    dGeomID id = 0;
    dSpaceID space = dGeomGetSpace(geom);
    switch (dGeomGetClass(geom))
    {
        case dSphereClass:
            id = dCreateSphere(space, dGeomSphereGetRadius(geom));
            break;
        case dBoxClass:
            {
                dVector3 size;
                dGeomBoxGetLengths(geom, size);
                id = dCreateBox(space, size[0], size[1], size[2]);
            }
            break;
        case dCCylinderClass:
            {
                dReal radius, length;
                dGeomCylinderGetParams(geom, &radius, &length);
                id = dCreateCylinder(space, radius, length);
            }
            break;
        case dCylinderClass:
            {
                dReal radius, length;
                dGeomCCylinderGetParams(geom, &radius, &length);
                id = dCreateCCylinder(space, radius, length);
            }
            break;
        case dPlaneClass:
            {
                dVector4 params;
                dGeomPlaneGetParams(geom, params);
                id = dCreatePlane(space,
                        params[0], params[1], params[2], params[3]);
            }
            break;
        case dGeomTransformClass:
            {
                dGeomID old_inner_geom = dGeomTransformGetGeom(geom);
                dGeomID new_inner_geom = cloneGeom(old_inner_geom, body);
                if (new_inner_geom != 0)
                {
                    id = dCreateGeomTransform(space);
                    dGeomTransformSetGeom(id, new_inner_geom);
                    dGeomTransformSetCleanup(id,
                            dGeomTransformGetCleanup(geom));
                    dGeomTransformSetInfo(id, dGeomTransformGetInfo(geom));
                }
            }
            break;
        case dRayClass:
        case dTriMeshClass:
        case dSimpleSpaceClass:
        case dHashSpaceClass:
            /* unsupported for cloning */
            break;
    }
    if (id != 0)
    {
        if (dGeomGetBody(geom) != 0)
        {
            /* if the original geom was attached to a body
             * (i.e., a non-static geom), then attach it to the new body */
            dGeomSetBody(id, body);
        }
        else if (dGeomGetClass(id) != dPlaneClass)
        {
            /* if the original geom was static, then copy the
             * position and rotation to the new geom */
            const dReal * pos = dGeomGetPosition(geom);
            const dReal * rot = dGeomGetRotation(geom);
            dGeomSetPosition(id, pos[0], pos[1], pos[2]);
            dGeomSetRotation(id, rot);
        }
    }
    return id;
}

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);
            }
        }
    }
    if (m_body != 0)
    {
        dMassTranslate(&m_mass, -m_mass.c[0], -m_mass.c[1], -m_mass.c[2]);
        dBodySetMass(m_body, &m_mass);
    }
}

void OdeWorld::Object::addCube(const vector<float> args)
{
    if (args.size() != 9)
        return;
    dGeomID id = dCreateBox(0,
            m_scale * args[0],
            m_scale * args[1],
            m_scale * args[2]);
    dMass mass;
    dMassSetBox(&mass, 1.0,
            m_scale * args[0],
            m_scale * args[1],
            m_scale * 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, m_scale * args[0]);
    dMass mass;
    dMassSetSphere(&mass, 1.0, m_scale * 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, m_scale * args[0], m_scale * args[1]);
    dMass mass;
    dMassSetCylinder(&mass, 1.0, 3, m_scale * args[0], m_scale * 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, m_scale * args[0], m_scale * args[1]);
    dMass mass;
    dMassSetCappedCylinder(&mass, 1.0, 3, m_scale * args[0], m_scale * 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 = m_scale * (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)
{
    locx = m_scale * locx;
    locy = m_scale * locy;
    locz = m_scale * locz;

    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);
        dMassAdd(&m_mass, mass);
    }
}

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

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

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);
    }
    m_position[0] = x;
    m_position[1] = y;
    m_position[2] = 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];
    }
    else
    {
        *x = m_position[0];
        *y = m_position[1];
        *z = m_position[2];
    }
}

void OdeWorld::Object::addForce(dReal fx, dReal fy, dReal fz)
{
    if (m_body != 0)
        dBodyAddForce(m_body, fx, fy, fz);
}

void OdeWorld::Object::addRelForce(dReal fx, dReal fy, dReal fz)
{
    if (m_body != 0)
        dBodyAddRelForce(m_body, fx, fy, fz);
}

void OdeWorld::Object::addTorque(dReal fx, dReal fy, dReal fz)
{
    if (m_body != 0)
        dBodyAddTorque(m_body, fx, fy, fz);
}

void OdeWorld::Object::addRelTorque(dReal fx, dReal fy, dReal fz)
{
    if (m_body != 0)
        dBodyAddRelTorque(m_body, fx, fy, fz);
}