/*!
  \file
  \brief ロボットの移動サンプル

  直進と旋回を行い、そのときの軌跡を出力する。

  \author Satofumi KAMIMURA

  $Id: robotMove.cpp 1124 2009-07-08 13:34:54Z satofumi $
*/

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

using namespace std;


namespace
{
  enum {
    Right = 0,
    Left = 1,

    Front = 0,
    Back = 1,

    X = 0,
    Y = 1,
    Z = 2,
  };


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


  typedef struct
  {
    Object body;
    Object drive_wheel[2];
    dJointID drive_joint[2];
    Object free_wheel[2];
    dJointID free_joint[2];
  } robot_t;


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

  const dReal RobotWeight = 3.0; // [Kg]
  const dReal RobotLength[] = { 0.4, 0.4, 0.2 };
  const dReal FirstPosition[] = { 0.0, 0.0, 0.2 };

  const dReal DriveWheelMass = 0.2;  // [Kg]
  const dReal DriveWheelRadius = 0.11; // [m]
  const dReal DriveWheelWidth = 0.02;   // [m]

  const dReal FreeWheelMass = 0.1;    // [Kg]
  const dReal FreeWheelRadius = 0.05; // [m]
  const dReal FreeWheelWidth = 0.02;   // [m]

  dReal WheelVelocity[2] = { 0.0, 0.0 };

  robot_t robot_;

  dsFunctions fn_;


  void createRobot(void)
  {
    // 筐体
    robot_.body.body = dBodyCreate(world_);
    dMass mass;
    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass, RobotWeight,
                     RobotLength[X], RobotLength[Y], RobotLength[Z]);
    dBodySetMass(robot_.body.body, &mass);
    robot_.body.geometry =
      dCreateBox(space_, RobotLength[X], RobotLength[Y], RobotLength[Z]);
    dGeomSetBody(robot_.body.geometry, robot_.body.body);
    dBodySetPosition(robot_.body.body,
                     FirstPosition[X], FirstPosition[Y], FirstPosition[Z]);

    // 車輪
    dMatrix3 R;
    dRFromAxisAndAngle(R, 0, 1, 0, M_PI / 2.0);
    for (int i = 0; i < 2; ++i) {
      robot_.drive_wheel[i].body = dBodyCreate(world_);
      dMassSetZero(&mass);
      dMassSetCylinderTotal(&mass, DriveWheelMass, 2, DriveWheelRadius,
                            DriveWheelWidth);
      dBodySetMass(robot_.drive_wheel[i].body, &mass);
      robot_.drive_wheel[i].geometry =
        dCreateCylinder(space_, DriveWheelRadius, DriveWheelWidth);
      dGeomSetBody(robot_.drive_wheel[i].geometry, robot_.drive_wheel[i].body);
      dBodySetRotation(robot_.drive_wheel[i].body, R);
    }

    dReal side = (RobotLength[X] + DriveWheelWidth) / 2.0;
    dReal drive_z = FirstPosition[Z] - (RobotLength[Z] / 2.0);
    dBodySetPosition(robot_.drive_wheel[Right].body, +side, 0, drive_z);
    dBodySetPosition(robot_.drive_wheel[Left].body, -side, 0, drive_z);

#if 1
    // 補助輪
    for (int i = 0; i < 2; ++i) {
      robot_.free_wheel[i].body = dBodyCreate(world_);
      dMassSetZero(&mass);
      dMassSetCylinderTotal(&mass, FreeWheelMass, 2, FreeWheelRadius,
                            FreeWheelWidth);
      dBodySetMass(robot_.free_wheel[i].body, &mass);
      robot_.free_wheel[i].geometry =
        dCreateCylinder(space_, FreeWheelRadius, FreeWheelWidth);
      dGeomSetBody(robot_.free_wheel[i].geometry, robot_.free_wheel[i].body);
      dBodySetRotation(robot_.free_wheel[i].body, R);
    }

    dReal front = (RobotLength[Y] / 2.0) - FreeWheelRadius;
    dReal back = -front;
    dReal free_z = FirstPosition[Z] - (RobotLength[Z] / 2.0) - FreeWheelRadius;
    dBodySetPosition(robot_.free_wheel[Front].body, 0, front, free_z);
    dBodySetPosition(robot_.free_wheel[Back].body, 0, back, free_z);
#endif

    // ジョイント、モータ
    for (int i = 0; i < 2; ++i) {
      robot_.drive_joint[i] = dJointCreateHinge(world_, 0);
      dJointAttach(robot_.drive_joint[i], robot_.body.body,
                   robot_.drive_wheel[i].body);
      dJointSetHingeAxis(robot_.drive_joint[i], 1, 0, 0);
    }
    dJointSetHingeAnchor(robot_.drive_joint[Right], +side, 0, drive_z);
    dJointSetHingeAnchor(robot_.drive_joint[Left], -side, 0, drive_z);

#if 1
    for (int i = 0; i < 2; ++i) {
      robot_.free_joint[i] = dJointCreateHinge(world_, 0);
      dJointAttach(robot_.free_joint[i], robot_.body.body,
                   robot_.free_wheel[i].body);
      dJointSetHingeAxis(robot_.free_joint[i], 1, 0, 0);
    }

    dJointSetHingeAnchor(robot_.free_joint[Front], 0, front, free_z);
    dJointSetHingeAnchor(robot_.free_joint[Back], 0, back, free_z);
#endif
  }


  void setWheelVelocity(int id, dReal velocity)
  {
    WheelVelocity[id] = velocity;
  }


  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;
      contact[i].surface.bounce = 0.5;
      contact[i].surface.bounce_vel = 0.1;

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


  void controlWheel(void)
  {
    // !!! 単位系、不明
    dReal fMax = 100.0;

    for (int i = 0; i < 2; ++i) {
      dJointSetHingeParam(robot_.drive_joint[i], dParamVel, WheelVelocity[i]);
      dJointSetHingeParam(robot_.drive_joint[i], dParamFMax, fMax);
    }
  }


  void printPosition(void)
  {
    const dReal* position = dBodyGetPosition(robot_.body.body);
    const dReal* rotation = dBodyGetRotation(robot_.body.body);

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

#if 0
    fprintf(stderr, "\n");
    fprintf(stderr, "%f, %f, %f\n", rotation[0], rotation[1], rotation[2]);
    fprintf(stderr, "%f, %f, %f\n", rotation[4], rotation[5], rotation[6]);
    fprintf(stderr, "%f, %f, %f\n", rotation[8], rotation[9], rotation[10]);
#endif

    //double degree = 180.0 * atan2(rotation[0], rotation[1]) / M_PI;
    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);
    for (int i = 0; i < 2; ++i) {
      position = dBodyGetPosition(robot_.drive_wheel[i].body);
      rotation = dBodyGetRotation(robot_.drive_wheel[i].body);
      dsDrawCylinder(position, rotation, DriveWheelWidth, DriveWheelRadius);
    }

    for (int i = 0; i < 2; ++i) {
      position = dBodyGetPosition(robot_.free_wheel[i].body);
      rotation = dBodyGetRotation(robot_.free_wheel[i].body);
      dsDrawCylinder(position, rotation, FreeWheelWidth, FreeWheelRadius);
    }
  }


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

    //controlWheel();
    dSpaceCollide(space_, 0, &nearCallback);

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

    printPosition();
  }


  void initializeSimulation(void)
  {
    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);
  }


  void terminateSimulation(void)
  {
    dSpaceDestroy(space_);
    dWorldDestroy(world_);

    dCloseODE();
  }


  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 setDrawStuff(void)
  {
    fn_.version = DS_VERSION;
    fn_.start = &start;
    fn_.step = &stepSimulation;
    fn_.path_to_textures = ".";
  }
}


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

  initializeSimulation();

  // ロボットの作成
  createRobot();

  enum {
    SimulationTimes = 100,      // [1]
  };

  // !!! 単位系、未定
  const dReal Velocity = 0.002;

#if 0
  // 直進
  setWheelVelocity(Right, Velocity);
  setWheelVelocity(Left, Velocity);

#else
  // 回転
  setWheelVelocity(Right, +Velocity);
  setWheelVelocity(Left, -Velocity);
#endif

  controlWheel();

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

  terminateSimulation();
  return 0;
}
