#ifndef ODEWORLD_H
#define ODEWORLD_H

#include <ode/ode.h>
#include <vector>
#include <string>
#include <map>
#include "refptr/refptr.h"
#ifdef DEBUG_AMOTOR
#include <iostream>
using namespace std;
#endif

class OdeWorld
{
    public:
        enum GeomType { BOX, SPHERE, PLANE, CYLINDER, CAPSULE };

        class Object
        {
            public:
                Object(bool is_static, OdeWorld * ode_world,
                        dWorldID world, dSpaceID space, float scale = 1.0f);
                Object(const Object & orig);
                ~Object();

                void setPosition(double x, double y, double z);
                void getPosition(double * x, double * y, double * z);
                void setRotation(dReal x, dReal y, dReal z);
                const dReal * getPosition();
                const dReal * getRotation();
                bool addBox(refptr< std::vector<float> > args);
                bool addSphere(refptr< std::vector<float> > args);
                bool addCylinder(refptr< std::vector<float> > args);
                bool addCapsule(refptr< std::vector<float> > args);
                bool addPlane(refptr< std::vector<float> > args);
                void addForce(dReal fx, dReal fy, dReal fz);
                void addForceRel(dReal fx, dReal fy, dReal fz);
                void addTorque(dReal fx, dReal fy, dReal fz);
                void addTorqueRel(dReal fx, dReal fy, dReal fz);
                void finalize();
                dReal getMass() { return m_mass.mass; }
                void setMass(dReal newmass);
                void enableBody();
                dBodyID getBody() { return m_body; }
                void setGravityMode(bool enabled)
                {
                    if (m_body != 0)
                    {
                        dBodySetGravityMode(m_body, enabled);
                    }
                }
                void setUserData(void * user_data)
                {
                    m_user_data = user_data;
                }
                void * getUserData() { return m_user_data; }
                std::vector<dGeomID> & getGeoms() { return m_geoms; }

            protected:
                bool    m_is_static;
                dBodyID m_body;
                dMass   m_mass;
                dWorldID m_world;
                dSpaceID m_space;
                std::vector<dGeomID> m_geoms;
                float   m_scale;
                dReal   m_position[3];
                dReal   m_rotation[12];
                OdeWorld * m_ode_world;
                void *  m_user_data;

                void setupGeom(dGeomID geom, dMass * mass,
                        float locx, float locy, float locz,
                        float rotx, float roty, float rotz);
                dGeomID cloneGeom(dGeomID geom, dBodyID body);
        };

        class PickedObject
        {
            public:
                Object * obj;
                float dist;
                float pos[3];
                float normal[3];

                PickedObject(dGeomID geom, dContactGeom & contact)
                {
                    obj = (Object *) dGeomGetData(geom);
                    dist = contact.depth;
                    pos[0] = contact.pos[0];
                    pos[1] = contact.pos[1];
                    pos[2] = contact.pos[2];
                    normal[0] = contact.normal[0];
                    normal[1] = contact.normal[1];
                    normal[2] = contact.normal[2];
                }
        };
        typedef refptr<PickedObject> PickedObjectRef;

        OdeWorld();
        ~OdeWorld();

        Object * createObject(bool is_static, float scale = 1.0f);
        void setGravity(float x, float y, float z)
        {
            dWorldSetGravity(m_world, x, y, z);
            enableAllBodies();
        }
        void step();
        void enableAllBodies();
        dWorldID getWorldID() { return m_world; }
        dJointID createAMotor(dBodyID b1, dBodyID b2);
        dJointID createHinge(dBodyID b1, dBodyID b2,
                dReal anchor_x, dReal anchor_y, dReal anchor_z,
                dReal axis_x, dReal axis_y, dReal axis_z);
        void setAMotorNumAxes(dJointID j, int num_axes)
        {
#ifdef DEBUG_AMOTOR
            cout << "num axes: " << num_axes << endl;
#endif
            dJointSetAMotorNumAxes(j, num_axes);
        }
        void setAMotorAngle(dJointID j, int anum, dReal val)
        {
#ifdef DEBUG_AMOTOR
            cout << "angle: " << val << endl;
#endif
            dJointSetAMotorAngle(j, anum, val);
        }
        void setAMotorAxis(dJointID j, int anum, int rel,
                dReal x, dReal y, dReal z)
        {
#ifdef DEBUG_AMOTOR
            cout << "axis: " << anum << ", " << rel
                << ", (" << x << ", " << y << ", " << z << ")" << endl;
#endif
            dJointSetAMotorAxis(j, anum, rel, x, y, z);
        }
        void setAMotorLoStop(dJointID j, dReal val)
        {
#ifdef DEBUG_AMOTOR
            cout << "lo stop: " << val << endl;
#endif
            dJointSetAMotorParam(j, dParamLoStop, val);
        }
        void setAMotorHiStop(dJointID j, dReal val)
        {
#ifdef DEBUG_AMOTOR
            cout << "hi stop: " << val << endl;
#endif
            dJointSetAMotorParam(j, dParamHiStop, val);
        }
        void setAMotorVel(dJointID j, dReal val)
        {
#ifdef DEBUG_AMOTOR
            cout << "vel: " << val << endl;
#endif
            dJointSetAMotorParam(j, dParamVel, val);
        }
        void setAMotorFMax(dJointID j, dReal val)
        {
#ifdef DEBUG_AMOTOR
            cout << "fmax: " << val << endl;
#endif
            dJointSetAMotorParam(j, dParamFMax, val);
        }
        void setAMotorBounce(dJointID j, dReal val)
        {
#ifdef DEBUG_AMOTOR
            cout << "bounce: " << val << endl;
#endif
            dJointSetAMotorParam(j, dParamBounce, val);
        }
        refptr< std::vector<PickedObjectRef> > pickObjects(
                float start_x, float start_y, float start_z,
                float dir_x, float dir_y, float dir_z);
        PickedObjectRef pickOne(
                float start_x, float start_y, float start_z,
                float dir_x, float dir_y, float dir_z,
                Object * o);

        friend void OdeWorld_collide_callback(void * data,
                dGeomID o1, dGeomID o2);

        friend void OdeWorld_pick_collide_callback(void * data,
                dGeomID o1, dGeomID o2);

        static void pushTransform(const dReal * pos, const dReal * R);

    protected:
        dBodyID         createBody();
        void            destroyBody(dBodyID body);

        dWorldID        m_world;
        dSpaceID        m_space;
        dJointGroupID   m_contactJointGroup;
        std::map<dBodyID, int>  m_bodies;
        dGeomID         m_pick_ray;
        std::vector< refptr<PickedObject> > m_pick_points;
};

#endif