#ifndef ODEWORLD_H
#define ODEWORLD_H

#include <ode/ode.h>
#include <vector>
#include <string>
#include <map>

class OdeWorld
{
    public:
        OdeWorld();
        ~OdeWorld();

        class Object
        {
            public:
                Object(bool is_static, dWorldID world, dSpaceID space);

                void loadPhy(const std::string & path);
                void setPosition(double x, double y, double z);
                void getPosition(double * x, double * y, double * z);
                const dReal * getPosition();
                const dReal * getRotation();
                std::vector<dGeomID> loadPhy(const std::string & path,
                        dBodyID * body, bool is_static = false);
                void addCube(const std::vector<float> args);
                void addSphere(const std::vector<float> args);
                void addCylinder(const std::vector<float> args);
                void addCCylinder(const std::vector<float> args);
                void addPlane(const std::vector<float> args);

            protected:
                bool    m_is_static;
                dBodyID m_body;
                dWorldID m_world;
                dSpaceID m_space;
                std::vector<dGeomID> m_geoms;

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

        };

        Object * createObject(bool is_static);
        void setGravity(float x, float y, float z)
        {
            dWorldSetGravity(m_world, x, y, z);
        }
        void step();

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

        static void pushTransform(const float pos[3], const float R[12]);

    protected:
        dWorldID        m_world;
        dSpaceID        m_space;
        dJointGroupID   m_contactJointGroup;
        std::map<std::string, dGeomID> m_objects;
};

#endif