/*!
  \file
  \brief ビーゴの移動サンプル

  \author Satofumi KAMIMURA

  $Id: moveSample.cpp 1478 2009-11-01 14:13:31Z satofumi $
*/

#include "BeegoModel.h"
#include "robot_t.h"
#include "OdeHandler.h"
#include <drawstuff/drawstuff.h>
#include <ode/ode.h>

using namespace qrk;


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

  dBodyID robot_body_;
  dBodyID wheels_[2];
  dBodyID free_wheel_ball_;

  dsFunctions fn_;

  const dReal RobotLength[] = { 0.180, 0.250, 0.020 }; // [m]


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

    robot_body_ = model.objectId();
    for (int i = 0; i < 2; ++i) {
      wheels_[i] = model.wheelsId(i);
    }
    free_wheel_ball_ = model.freeWheelBallId();
  }


  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 start(void)
  {
    static float xyz[] = { 0.0, -1.00, 1.00 };
    static float hpr[] = { 80.0, -40.0, 0.0 };
    //static float hpr[] = { 0.0, -90.0, 0.0 };

    dsSetViewpoint(xyz, hpr);
  }


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

    dsSetColor(0.0, 0.0, 1.0);
    dsDrawBox(position, rotation, RobotLength);

    //double degree = 180.0 * atan2(-rotation[0], rotation[4]) / M_PI;
    //fprintf(stderr, "%f, %f, %f\n", position[0], position[1], degree);

    dsSetColor(1.0, 1.0, 1.0);
    const dReal DriveWheelRadius = WHEEL_T_RADIUS / 1000.0;
    const dReal DriveWheelWidth = 0.02;
    for (int i = 0; i < 2; ++i) {
      position = dBodyGetPosition(wheels_[i]);
      rotation = dBodyGetRotation(wheels_[i]);
      dsDrawCylinder(position, rotation, DriveWheelWidth, DriveWheelRadius);
    }

    dsSetColor(1.0, 1.0, 1.0);
    const dReal FreeWheelRadius = 0.030;
    position = dBodyGetPosition(free_wheel_ball_);
    rotation = dBodyGetRotation(free_wheel_ball_);
    dsDrawSphere(position, rotation, FreeWheelRadius);
  }


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

  OdeHandler ode_;
  BeegoModel beego_model;
  beego_model.activate();

  Position<long> first_position(0, 0, deg(0));
  beego_model.setPosition(first_position);

  initializeSimulation(ode_, beego_model);

  enum {
    Right = 0,
    Left = 1,
  };

  // 500 で 30 [mm/sec] 相当
  //const int velocity = 500;
  const int velocity = 0;

#if 1
  // 直進
  beego_model.setMotorVelocity(Right, velocity);
  beego_model.setMotorVelocity(Left, velocity);

#else
  // 回転
  beego_model.setMotorVelocity(Right, velocity);
  beego_model.setMotorVelocity(Left, -velocity);

#endif

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

  return 0;
}
