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

  \author Satofumi KAMIMURA

  $Id: cylinderSample.cpp 1155 2009-07-18 01:24:09Z satofumi $
*/

#include "Cylinder.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 cylinder_;

  dsFunctions fn_;

  const dReal Length = 0.800;   // [m]
  const dReal Radius = 0.300;   // [m]


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

    cylinder_ = 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(cylinder_);
    const dReal* rotation = dBodyGetRotation(cylinder_);

    dsSetColor(0.8, 0.8, 0.8);
    dsDrawCylinder(position, rotation, Length, Radius);
  }


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);

  Cylinder cylinder(static_cast<long>(Length * 1000),
                    static_cast<long>(Radius * 1000));

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

  OdeHandler ode_;
  initializeSimulation(ode_, cylinder);

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

  return 0;
}
