
#include "OdeWorld.h"
#include <vector>
#include <fstream>
#include <iostream>
#include <algorithm>                /* std::sort() */
#include <GL/gl.h>
using namespace std;

#define WORLD_STEP 0.001

/* used by ODE to perform collision detection */
void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2)
{
    const int maxNumContacts = 4;
    static dContact contact[maxNumContacts];
    OdeWorld * ow = (OdeWorld *) data;
    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);
    }
}

void OdeWorld_pick_collide_callback(void * data, dGeomID o1, dGeomID o2)
{
    dContactGeom contact_geom;
    OdeWorld * ow = (OdeWorld *) data;
    int num = dCollide(o1, o2, 1, &contact_geom, sizeof(contact_geom));
    if (num > 0)
    {
        dGeomID other = o1 == ow->m_pick_ray ? o2 : o1;
        ow->m_pick_points.push_back(
                new OdeWorld::PickedObject(other, contact_geom));
    }
}

OdeWorld::OdeWorld()
{
    m_world = dWorldCreate();
    dWorldSetAutoDisableFlag(m_world, 1);
    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, this, 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);
}

dBodyID OdeWorld::createBody()
{
    dBodyID id = dBodyCreate(m_world);
    m_bodies[id] = 1;
    return id;
}

void OdeWorld::enableAllBodies()
{
    for (std::map<dBodyID, int>::iterator it = m_bodies.begin();
            it != m_bodies.end();
            it++)
    {
        dBodyEnable(it->first);
    }
}

dJointID OdeWorld::createAMotor(dBodyID b1, dBodyID b2)
{
    dJointID j = dJointCreateAMotor(m_world, 0);
    dJointAttach(j, b1, b2);
    return j;
}

dJointID OdeWorld::createHinge(dBodyID b1, dBodyID b2,
        dReal anchor_x, dReal anchor_y, dReal anchor_z,
        dReal axis_x, dReal axis_y, dReal axis_z)
{
    dJointID j = dJointCreateHinge(m_world, 0);
    dJointAttach(j, b1, b2);
    dJointSetHingeAnchor(j, anchor_x, anchor_y, anchor_z);
    dJointSetHingeAxis(j, axis_x, axis_y, axis_z);
    return j;
}

static bool RPPickedObjectComparator(const refptr<OdeWorld::PickedObject> & one,
        const refptr<OdeWorld::PickedObject> & two)
{
    return one->dist < two->dist;
}

refptr< vector< OdeWorld::PickedObjectRef > > OdeWorld::pickObjects(
        float start_x, float start_y, float start_z,
        float dir_x, float dir_y, float dir_z)
{
    m_pick_ray = dCreateRay(0, dInfinity);
    dGeomRaySet(m_pick_ray, start_x, start_y, start_z, dir_x, dir_y, dir_z);
    m_pick_points.clear();
    dSpaceCollide2(m_pick_ray, (dGeomID) m_space,
            this, OdeWorld_pick_collide_callback);
    std::sort(m_pick_points.begin(), m_pick_points.end(),
            RPPickedObjectComparator);
    dGeomDestroy(m_pick_ray);
    return new vector<OdeWorld::PickedObjectRef>(m_pick_points);
}

OdeWorld::PickedObjectRef OdeWorld::pickOne(
        float start_x, float start_y, float start_z,
        float dir_x, float dir_y, float dir_z,
        OdeWorld::Object * o)
{
    dGeomID pick_ray = dCreateRay(0, dInfinity);
    dGeomRaySet(pick_ray, start_x, start_y, start_z, dir_x, dir_y, dir_z);

    PickedObjectRef por;
    bool found_one = false;

    dContactGeom contact_geom;
    for (vector<dGeomID>::iterator it = o->getGeoms().begin();
         it != o->getGeoms().end();
         it++)
    {
        dGeomID g = *it;
        int num = dCollide(pick_ray, g, 1, &contact_geom, sizeof(contact_geom));
        if (num > 0 && (!found_one || contact_geom.depth < por->dist))
        {
            por = new OdeWorld::PickedObject(g, contact_geom);
            found_one = true;
        }
    }
    dGeomDestroy(pick_ray);
    return por;
}

void OdeWorld::destroyBody(dBodyID body)
{
    m_bodies.erase(body);
    dBodyDestroy(body);
    enableAllBodies();
}

OdeWorld::Object::Object(bool is_static, OdeWorld * ode_world,
        dWorldID world, dSpaceID space, float scale)
{
    m_is_static = is_static;
    m_world = world;
    m_space = space;
    m_body = 0;
    m_scale = scale;
    m_ode_world = ode_world;
    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;
    m_ode_world = orig.m_ode_world;
    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 = m_ode_world->createBody();
        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)
        m_ode_world->destroyBody(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 dCylinderClass:
            {
                dReal radius, length;
                dGeomCylinderGetParams(geom, &radius, &length);
                id = dCreateCylinder(space, radius, length);
            }
            break;
        case dCapsuleClass:
            {
                dReal radius, length;
                dGeomCapsuleGetParams(geom, &radius, &length);
                id = dCreateCapsule(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);
        }
        dGeomSetData(id, dGeomGetData(geom));
    }
    return id;
}

bool OdeWorld::Object::addBox(refptr< std::vector<float> > args)
{
    if (args->size() != 9)
        return false;
    dGeomID id = dCreateBox(0,
            m_scale * (*args)[0],
            m_scale * (*args)[1],
            m_scale * (*args)[2]);
    dGeomSetData(id, this);
    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]);
    return true;
}

bool OdeWorld::Object::addSphere(refptr< std::vector<float> > args)
{
    if (args->size() != 4)
        return false;
    dGeomID id = dCreateSphere(0, m_scale * (*args)[0]);
    dGeomSetData(id, this);
    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);
    return true;
}

bool OdeWorld::Object::addCylinder(refptr< std::vector<float> > args)
{
    if (args->size() != 8)
        return false;
    dGeomID id = dCreateCylinder(0, m_scale * (*args)[0], m_scale * (*args)[1]);
    dGeomSetData(id, this);
    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]);
    return true;
}

bool OdeWorld::Object::addCapsule(refptr< std::vector<float> > args)
{
    if (args->size() != 8)
        return false;
    dGeomID id = dCreateCapsule(0,
            m_scale * (*args)[0], m_scale * (*args)[1]);
    dGeomSetData(id, this);
    dMass mass;
    dMassSetCapsule(&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]);
    return true;
}

bool OdeWorld::Object::addPlane(refptr< std::vector<float> > args)
{
    float a, b, c, d;
    if (args->size() == 6)
    {
        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);

        a = rotated_plane_direction[0];
        b = rotated_plane_direction[1];
        c = rotated_plane_direction[2];
        d = m_scale * (a * (*args)[0] + b * (*args)[1] + c * (*args)[2]);
    }
    else if (args->size() == 4)
    {
        a = (*args)[0];
        b = (*args)[1];
        c = (*args)[2];
        d = (*args)[3];
    }
    else
    {
        return false;
    }

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

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);
    dGeomSetData(transform, this);

    m_geoms.push_back(transform);

    if (!m_is_static)
    {
        /* attach the geometry to the body */
        if (m_body == 0)
            m_body = m_ode_world->createBody();

        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)
    {
        if (dGeomGetClass(m_geoms[0]) != dPlaneClass)
            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)
    {
        if (dGeomGetClass(m_geoms[0]) != dPlaneClass)
            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);
            enableBody();
        }
    }
    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)
        {
            if (dGeomGetClass(m_geoms[0]) == dPlaneClass)
                pos = &m_position[0];
            else
                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::setRotation(dReal x, dReal y, dReal z)
{
    dMatrix3 r;
    dRFromEulerAngles(r, x, y, z);
    if (m_is_static)
    {
        for (int i = 0, sz = m_geoms.size(); i < sz; i++)
            dGeomSetRotation(m_geoms[i], r);
    }
    else
    {
        if (m_body != 0)
        {
            dBodySetRotation(m_body, r);
            enableBody();
        }
    }
}

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

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

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

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

void OdeWorld::Object::finalize()
{
    if (m_body != 0)
    {
#if 0
        cerr << "Warning: translating mass by ("
            << (-m_mass.c[0]) << ", "
            << (-m_mass.c[1]) << ", "
            << (-m_mass.c[2]) << ")" << endl;
#endif
        dMassTranslate(&m_mass, -m_mass.c[0], -m_mass.c[1], -m_mass.c[2]);
        dBodySetMass(m_body, &m_mass);
    }
}

void OdeWorld::Object::setMass(dReal newmass)
{
    if (m_body != 0)
    {
        dMassAdjust(&m_mass, newmass);
        dBodySetMass(m_body, &m_mass);
        enableBody();
    }
}

void OdeWorld::Object::enableBody()
{
    if (m_body != 0)
    {
        dBodyEnable(m_body);
    }
}
