/*!
  \file
  \brief Box の配置サンプル

  \author Satofumi KAMIMURA

  $Id: boxSample.cpp 1152 2009-07-17 13:40:27Z satofumi $
*/

#include "Box.h"
#include "OdeHandler.h"
#include <drawstuff/drawstuff.h>

using namespace qrk;


namespace
{
  enum {
    X = 0,
    Y = 1,
    Z = 2,
  };

  dWorldID world_;
  dSpaceID space_;
  dGeomID ground_;
  dJointGroupID contact_group_;

  dBodyID box_;

  dsFunctions fn_;

  const dReal BoxLength[] = { 0.300, 0.400, 0.800 }; // [m]


  void initializeSimulation(OdeHandler& ode, Box& model)
  {
    world_ = ode.world();
    space_ = ode.space();
    ground_ = ode.ground();
    contact_group_ = ode.contactGroup();

    box_ = model.objectId();
  }


  void start(void)
  {
    static float xyz[] = { 0.0, -2.00, 2.00 };
    static float hpr[] = { 80.0, -40.0, 0.0 };

    dsSetViewpoint(xyz, hpr);
  }


  void drawModel(void)
  {
    const dReal* position = dBodyGetPosition(box_);
    const dReal* rotation = dBodyGetRotation(box_);

    dsSetColor(0.0, 1.0, 0.0);
    dsDrawBox(position, rotation, BoxLength);
  }


void nearCallback(void* data, dGeomID o1, dGeomID o2)
{
  static_cast<void>(data);

  enum { N = 10 };            // 接触点数の最大値
  dContact contact[N];

  int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
  bool is_ground = (ground_ == o1) || (ground_ == o2);
  if (! is_ground) {
    return;
  }

  for (int i = 0; i < n; ++i) {
    contact[i].surface.mode =
      dContactBounce | dContactSoftCFM | dContactSoftERP;
    contact[i].surface.bounce = 0.5;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
    contact[i].surface.soft_erp = 0.1;
    contact[i].surface.mu = 2.0;

    dJointID c = dJointCreateContact(world_, contact_group_, &contact[i]);
    dJointAttach(c,
                 dGeomGetBody(contact[i].geom.g1),
                 dGeomGetBody(contact[i].geom.g2));
  }
}


  void stepSimulation(int pause)
  {
    static_cast<void>(pause);

    dSpaceCollide(space_, 0, &nearCallback);

    dWorldStep(world_, 0.001);
    dJointGroupEmpty(contact_group_);

    drawModel();
  }



  void setDrawStuff(void)
  {
    fn_.version = DS_VERSION;
    fn_.start = &start;
    fn_.step = &stepSimulation;
    fn_.path_to_textures = "../odeSamples";
  }
}


int main(int argc, char *argv[])
{
  static_cast<void>(argc);
  static_cast<void>(argv);

  Box box(static_cast<long>(BoxLength[X] * 1000),
          static_cast<long>(BoxLength[Y] * 1000),
          static_cast<long>(BoxLength[Z] * 1000));

  Position<long> first_position(1000, 0, deg(30));
  box.setPosition(first_position);

  OdeHandler ode_;
  initializeSimulation(ode_, box);

  setDrawStuff();
  dsSimulationLoop(argc, argv, 352, 288, &fn_);

  return 0;
}
