extern "C" {
  #include "screen.h"
  #include "event.h"
  #include "util.h"
  #include "block.h"
}

#include <ode/ode.h>

#define TYPE_STATIC  0x00
#define TYPE_MOVABLE 0x01

struct object {
  unsigned char type;
  dGeomID geom;
  dBodyID body;
  struct block block;
};

struct screen screen;
dWorldID world;
dSpaceID space;
dJointGroupID contactgroup;

const char progname[] = "test1";

struct object objects[] = {
  { TYPE_STATIC,  0, 0, { "base1", 8.0, 1.0, 2.0, 10.0, 0, 0} },
  { TYPE_MOVABLE, 0, 0, { "drop",  1.0, 1.0, 1.2, 1.0,  0, 0} },
};

void init_ode()
{
  dMass dm;
  unsigned int ind;

  info("init_ode", "ode init");

  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);

  dWorldSetGravity(world, 0, 9.81, 0);
  dWorldSetAutoDisableFlag(world, 1);
  dWorldSetContactMaxCorrectingVel(world, 0.1);

  for (ind = 0; ind < sizeof(objects) / sizeof(struct object); ++ind) {
    struct object *o = &objects[ind];
    o->body = 0;
    o->geom = dCreateBox(space, o->block.w, o->block.h, 1.0);
    if (o->type == TYPE_MOVABLE) {
      o->body = dBodyCreate(world);
      dBodySetPosition(o->body, o->block.x, o->block.y, 0.0);
      dMassSetZero(&dm);
      dMassSetBoxTotal(&dm, 1.0, o->block.w, o->block.h, 1.0);
      dBodySetMass(o->body, &dm);
    }
    dGeomSetPosition(o->geom, o->block.x, o->block.y, 0.0);
    dGeomSetBody(o->geom, o->body);
  }
}
void close_ode()
{
  info("close_ode", "ode close");

  dJointGroupDestroy(contactgroup);
  dSpaceDestroy(space);
  dWorldDestroy(world);
}

void near_callback(void *data, dGeomID o1, dGeomID o2)
{
  dContact contact[8];
  unsigned int ind;
  unsigned int nc;
  dBodyID b1;
  dBodyID b2;

  b1 = dGeomGetBody(o1);
  b2 = dGeomGetBody(o2);

  /* do nothing if the two bodies are connected by a joint */
  if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
    return;

  for (ind = 0; ind < 8; ++ind) {
    contact[ind].surface.mode = dContactBounce | dContactSoftCFM;
    contact[ind].surface.mu = 0;
    contact[ind].surface.mu2 = 0;
    contact[ind].surface.bounce = 0.1;
    contact[ind].surface.bounce_vel = 0.1;
    contact[ind].surface.soft_cfm = 0.01;
  }
  nc = dCollide(o1, o2, 8, &contact[0].geom, sizeof(dContact));

  /* now what? */
  if (nc) {
    const dReal *xyz;
    const dReal *rot;
    xyz = dGeomGetPosition(o1);
    rot = dGeomGetRotation(o1);
    printf("o1    %f %f %f  %f %f %f\n",
                  xyz[0], xyz[1], xyz[2],
                  rot[0], rot[1], rot[2]);
    xyz = dGeomGetPosition(o2);
    rot = dGeomGetRotation(o2);
    printf("o2    %f %f %f  %f %f %f\n",
                  xyz[0], xyz[1], xyz[2],
                  rot[0], rot[1], rot[2]);
  }

  for (ind = 0; ind < nc; ++ind) {
    dJointID cnt;
    cnt = dJointCreateContact(world, contactgroup, contact + ind);
    dJointAttach(cnt, b1, b2);
  }
}
void move()
{
  const dReal *xyz;
  const dReal *rot;
  unsigned int ind;

  dSpaceCollide(space, 0, &near_callback);  
  dWorldQuickStep(world, 0.01);

  dJointGroupEmpty(contactgroup);

  for (ind = 0; ind < sizeof(objects) / sizeof(struct object); ++ind) {
    struct object *o = &objects[ind];
    xyz = dGeomGetPosition(o->geom);
    rot = dGeomGetRotation(o->geom);
    o->block.x = xyz[0];
    o->block.y = xyz[1];
  }
}
void draw()
{
  unsigned int ind;

  glClearColor(1.0, 1.0, 1.0, 1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glColor3f(0.0, 0.0, 0.0);

  glLoadIdentity();

  for (ind = 0; ind < sizeof(objects) / sizeof(struct object); ++ind)
    block_draw(&objects[ind].block);

  SDL_GL_SwapBuffers();
}

int main()
{
  SDL_Event event;

  init_ode();
  if (atexit(close_ode) == -1)
    die("atexit", 0);

  screen.rect.w = 640;
  screen.rect.h = 480;
  screen.bpp = 32;

  if (!screen_init(&screen))
    die("screen_init", screen_error());
  if (atexit(screen_end) == -1)
    die("atexit", 0);
  if (!event_init())
    die("event_init", event_error());
  if (atexit(event_end) == -1)
    die("atexit", 0);

  init_gl();

  for (;;) {
    while (event_poll(&event)) {
      switch (event.type) { 
        case SDL_KEYUP:
          switch (event.key.keysym.sym) {
            case SDLK_ESCAPE: quit(); break;
            default: break;
          }
          break;
        case SDL_QUIT: quit(); break;
        default: break;
      }
    }
    move();
    draw();
    SDL_Delay(10);
  }

  close_ode();
  return 0;
}