/*!
  \file
  \brief レーザの距離計測サンプル

  物体までの距離計測を行う。

  \author Satofumi KAMIMURA

  $Id: laserLength.cpp 1113 2009-07-06 21:58:15Z satofumi $
*/

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <iostream>

using namespace std;


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

  dsFunctions fn_;


  typedef struct {
    dBodyID body;
    dGeomID geometry;
    dReal length;
    dReal mass;
  } Object;

  const dReal DefaultRayLength = 30.0;
  const dReal BoxLength = 1.0;
  const dReal LaserHeight = 0.1;

  Object box_;
  dGeomID ray_;
  dReal ray_length_ = DefaultRayLength;


  void createBox(void)
  {
    box_.body = dBodyCreate(world_);

    const dReal weight = 1.0;

    dMass mass;
    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass, weight, BoxLength, BoxLength, BoxLength);
    dBodySetMass(box_.body, &mass);
    dBodySetPosition(box_.body, 5.0, 0.0, 2.0);
    box_.length = BoxLength;
    box_.geometry = dCreateBox(space_, box_.length, box_.length, box_.length);
    dGeomSetBody(box_.geometry, box_.body);
  }


  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);
    for (int i = 0; i < n; ++i) {

      if ((o1 == ray_) || (o2 == ray_)) {
        ray_length_ = sqrt(pow(contact[0].geom.pos[0], 2) +
                           pow(contact[0].geom.pos[1], 2) +
                           pow(contact[0].geom.pos[2] - LaserHeight, 2));
        return;
      }

      if (is_ground) {
        contact[i].surface.mode = dContactBounce;
        contact[i].surface.bounce = 0.5;
        contact[i].surface.bounce_vel = 0.01;

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


  void start(void)
  {
    static float xyz[] = { 0.0, -3.0, 1.0 };
    static float hpr[] = { 60.0, 0.0, 0.0 };

    dsSetViewpoint(xyz, hpr);
  }


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

    dSpaceCollide(space_, 0, &nearCallback);

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

    dsSetColor(0.0, 0.0, 1.0);
    const dReal* position = dBodyGetPosition(box_.body);
    const dReal* rotation = dBodyGetRotation(box_.body);

    dReal lines[] = { BoxLength, BoxLength, BoxLength };
    dsDrawBox(position, rotation, lines);

    dsSetColor(1.0, 0.0, 0.0);
    dReal p1[] = { 0.0, 0.0, LaserHeight };
    dReal p2[] = { 30.0, 0.0, LaserHeight };
    dsDrawLine(p1, p2);

    printf("%.2f [m]\n", ray_length_);
    //cout << ray_length_ << " [m]" << endl;
    ray_length_ = DefaultRayLength;
  }


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


int main(int argc, char *argv[])
{
  dInitODE();

  world_ = dWorldCreate();
  dWorldSetGravity(world_, 0.0, 0.0, -9.8);
  space_ = dHashSpaceCreate(0);
  contact_group_ = dJointGroupCreate(0);
  ground_ = dCreatePlane(space_, 0, 0, 1, 0);

  // 物体の配置
  createBox();

  // Ray の配置
  ray_ = dCreateRay(space_, DefaultRayLength);
  dGeomRaySet(ray_, 0.0, 0.0, LaserHeight, 1.0, 0.0, 0.0);

  // シミュレーション
  setDrawStuff();
  dsSimulationLoop(argc, argv, 352, 288, &fn_);

  dSpaceDestroy(space_);
  dWorldDestroy(world_);
  dCloseODE();

  return 0;
}
