/*!
  \file
  \brief ビーゴの ODE 用モデル

  \author Satofumi KAMIMURA

  $Id: BeegoModel.cpp 1619 2010-01-15 14:29:21Z satofumi $
*/

#include "BeegoModel.h"
#include "robot_t.h"
#include "encoder_control.h"
#include "OdeHandler.h"
#include "ModelManager.h"
#include "Rotate.h"

using namespace qrk;
using namespace std;


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

    Front = 0,
    Back = 1,

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


  const dReal RobotWeight = 5.00;                      // [Kg]
  const dReal RobotLength[] = { 0.180, 0.275, 0.020 }; // [m]
  const dReal FirstPosition[] = { 0.0, 0.0, 0.2 };     // [m]

  const dReal DriveWheelMass = 0.20;                    // [Kg]
  const dReal DriveWheelRadius = WHEEL_T_RADIUS / 1000.0; // [m]
  const dReal DriveWheelWidth = 0.010;                  // [m]
  const dReal WheelHeightOffset = 0.015;                // [m]

  const dReal FreeWheelMass = 0.10;    // [Kg]
  const dReal FreeWheelRadius = 0.038; // [m]
  const dReal FreeWheelBackOffset = 0.055; // [m]

  const dReal WheelSide = ((RobotLength[Y] + DriveWheelWidth) / 2.0) + 0.020;
  const dReal WheelFront = 0.070;
  const dReal WheelFrontZ = WheelHeightOffset;
  const dReal WheelBack = (RobotLength[X] / 2.0) + FreeWheelBackOffset;
  const dReal WheelBackZ = 0.010;

  const long WheelEncoderCount[2] = {
    static_cast<long>(ENCODER_T_COUNT * WHEEL_T_GEAR_RATIO),
    static_cast<long>(ENCODER_T_COUNT * WHEEL_T_GEAR_RATIO),
  };


  typedef struct
  {
    dBodyID body;
    dGeomID geometry;
  } object_t;


  typedef struct
  {
    object_t body;

    object_t drive_wheel[2];
    dJointID drive_joint[2];

    object_t free_wheel_ball;
    dJointID free_joint;
  } beego_t;
}


struct BeegoModel::pImpl
{
  OdeHandler ode_;
  dWorldID world_;
  dSpaceID space_;

  enum {
    BodyModel,
    RightWheelModel,
    LeftWheelModel,
    FreeWheelModel,
  };
  ModelManager::model_t models_[4];

  beego_t beego_;
  dReal wheel_max_radius_;

  double wheel_radius_[2];
  double gear_ratio_[2];

  long previous_encoder_count_[2];


  pImpl(void)
    : world_(ode_.world()), space_(ode_.space()),
      wheel_max_radius_(DriveWheelRadius)
  {
    for (int i = 0; i < 2; ++i) {
      wheel_radius_[i] = WHEEL_T_RADIUS / 1000.0;
      gear_ratio_[i] = WHEEL_T_GEAR_RATIO;
      previous_encoder_count_[i] = 0;
    }
    wheel_max_radius_ = max(wheel_radius_[0], wheel_radius_[1]);
  }


  void createModel(void)
  {
    // 筐体
    beego_.body.body = dBodyCreate(world_);

    dMass mass;
    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass, RobotWeight,
                     RobotLength[X], RobotLength[Y], RobotLength[Z]);
    dBodySetMass(beego_.body.body, &mass);
    beego_.body.geometry =
      dCreateBox(space_, RobotLength[X], RobotLength[Y], RobotLength[Z]);
    dGeomSetBody(beego_.body.geometry, beego_.body.body);
    dBodySetPosition(beego_.body.body,
                     FirstPosition[X], FirstPosition[Y], FirstPosition[Z]);

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

    dBodySetPosition(beego_.drive_wheel[Right].body,
                     WheelFront, -WheelSide, FirstPosition[Z] + WheelFrontZ);
    dBodySetPosition(beego_.drive_wheel[Left].body,
                     WheelFront, +WheelSide, FirstPosition[Z] + WheelFrontZ);

    // 補助輪
    beego_.free_wheel_ball.body = dBodyCreate(world_);
    dMassSetZero(&mass);
    dMassSetSphereTotal(&mass, FreeWheelMass, FreeWheelRadius);
    dBodySetMass(beego_.free_wheel_ball.body, &mass);
    beego_.free_wheel_ball.geometry =
      dCreateSphere(space_, FreeWheelRadius);
    dGeomSetBody(beego_.free_wheel_ball.geometry, beego_.free_wheel_ball.body);

    dBodySetPosition(beego_.free_wheel_ball.body,
                     -WheelBack, 0, FirstPosition[Z] + WheelBackZ);

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

    beego_.free_joint = dJointCreateBall(world_, 0);
    dJointAttach(beego_.free_joint, beego_.body.body,
                 beego_.free_wheel_ball.body);
    dJointSetBallAnchor(beego_.free_joint, -WheelBack, 0.0,
                        FirstPosition[Z] + WheelBackZ);


    for (int i = 0; i < 2; ++i) {
      // 最大トルク [Nm] の制限
      dReal fMax = 100.0;
      dJointSetHingeParam(beego_.drive_joint[i], dParamFMax, fMax);

      // 跳ねないようにするための調整
      dJointSetHingeParam(beego_.drive_joint[i], dParamFudgeFactor, 0.8);
    }

    // モデルの登録
    registerModel();
  }


  void registerModel(void)
  {
    Color body_color(1.0 * 0x46 / 0xff, 1.0 * 0x8c / 0xff, 1.0 * 0xc6 / 0xff);
    Color wheel_color(0.7, 0.7, 0.7);

    ModelManager model_manager;

    models_[BodyModel].type = ModelManager::Box;
    models_[BodyModel].body_id = beego_.body.body;
    models_[BodyModel].color = body_color;
    models_[BodyModel].obstacle = false;
    model_manager.addBox(&models_[BodyModel], RobotLength);

    models_[RightWheelModel].type = ModelManager::Cylinder;
    models_[RightWheelModel].body_id = beego_.drive_wheel[Right].body;
    models_[RightWheelModel].color = wheel_color;
    models_[RightWheelModel].obstacle = false;
    model_manager.addCylinder(&models_[RightWheelModel],
                              DriveWheelRadius, DriveWheelWidth);

    models_[LeftWheelModel].type = ModelManager::Cylinder;
    models_[LeftWheelModel].body_id = beego_.drive_wheel[Left].body;
    models_[LeftWheelModel].color = wheel_color;
    models_[LeftWheelModel].obstacle = false;
    model_manager.addCylinder(&models_[LeftWheelModel],
                              DriveWheelRadius, DriveWheelWidth);

    models_[FreeWheelModel].type = ModelManager::Sphere;
    models_[FreeWheelModel].body_id = beego_.free_wheel_ball.body;
    models_[FreeWheelModel].color = wheel_color;
    models_[FreeWheelModel].obstacle = false;
    model_manager.addSphere(&models_[FreeWheelModel], FreeWheelRadius);
  }


  void setMotorVelocity(int id, int count_per_msec)
  {
    double rad_per_sec =
      (2.0 * M_PI * (-count_per_msec / 32768.0) * 1000.0) / gear_ratio_[id];

    // モータの目標角速度 [rad/sec] の指定
    dJointSetHingeParam(beego_.drive_joint[id], dParamVel, rad_per_sec);
  }
};


BeegoModel::BeegoModel(void) : pimpl(new pImpl)
{
}


BeegoModel::~BeegoModel(void)
{
}


void BeegoModel::activate(void)
{
  pimpl->createModel();
}


void BeegoModel::setPosition(const Position<long>& position,
                             OdeModel* model, bool fixed)
{
  // 指定角度に回転
  double radian = position.to_rad();
  dMatrix3 R_body;
  dRFromAxisAndAngle(R_body, 0.0, 0.0, 1.0, radian);
  dGeomSetRotation(pimpl->beego_.body.geometry, R_body);

  dMatrix3 R_wheel;
  Position<dReal> offset(-1.0, 0.0, deg(0));
  offset = rotate<dReal>(offset, rad(radian + (M_PI / 2.0)));
  dRFromZAxis(R_wheel, offset.x, offset.y, 0.0);
  dGeomSetRotation(pimpl->beego_.drive_wheel[0].geometry, R_wheel);
  dGeomSetRotation(pimpl->beego_.drive_wheel[1].geometry, R_wheel);

  // 両輪の間が setPosition() の位置になるようにする
  dReal x = (position.x / 1000.0) - (WheelFront * cos(radian));
  dReal y = (position.y / 1000.0) - (WheelFront * sin(radian));

  // 車輪位置よりも 10 [mm] 程度高い位置に配置するようにする
  dReal z = (pimpl->wheel_max_radius_ - WheelHeightOffset) + 0.010;
  dGeomSetPosition(pimpl->beego_.body.geometry, x, y, z);

  offset = Position<dReal>(WheelFront, -WheelSide, deg(0));
  offset = rotate(offset, rad(radian));
  dGeomSetPosition(pimpl->beego_.drive_wheel[0].geometry,
                   x + offset.x, y + offset.y, z + WheelFrontZ);

  offset = Position<dReal>(WheelFront, +WheelSide, deg(0));
  offset = rotate(offset, rad(radian));
  dGeomSetPosition(pimpl->beego_.drive_wheel[1].geometry,
                   x + offset.x, y + offset.y, z + WheelFrontZ);

  offset = Position<dReal>(-WheelBack, 0, deg(0));
  offset = rotate(offset, rad(radian));
  dGeomSetPosition(pimpl->beego_.free_wheel_ball.geometry,
                   x + offset.x, y + offset.y, z + WheelBackZ);

  // !!! fixed の処理
  (void)model;
  (void)fixed;
  // !!!
}


Position<long> BeegoModel::position(void) const
{
  const dReal* position = dBodyGetPosition(pimpl->beego_.body.body);
  const dReal* rotation = dBodyGetRotation(pimpl->beego_.body.body);
  double radian = atan2(-rotation[0], rotation[4]) + (M_PI / 2.0);

  // WheelFront だけずらした位置を返す
  dReal x = position[X] + (WheelFront * cos(radian));
  dReal y = position[Y] + (WheelFront * sin(radian));
  // !!!

  return Position<long>(static_cast<long>(1000.0 * x),
                        static_cast<long>(1000.0 * y), rad(radian));
}


void BeegoModel::setPosition3d(long px, long py, long pz,
                               double ax, double ay, double az,
                               const Angle& angle,
                               OdeModel* model, bool fixed)
{
  (void)px;
  (void)py;
  (void)pz;
  (void)ax;
  (void)ay;
  (void)az;
  (void)angle;

  (void)model;
  (void)fixed;

  // !!!
}


void BeegoModel::position3d(long& px, long& py, long& pz,
                            double& dx, double& dy, double& dz) const
{
  (void)px;
  (void)py;
  (void)pz;
  (void)dx;
  (void)dy;
  (void)dz;

  // !!!
}


dBodyID BeegoModel::objectId(void) const
{
  return pimpl->beego_.body.body;
}


dBodyID BeegoModel::wheelsId(int id) const
{
  return pimpl->beego_.drive_wheel[id].body;
}


dBodyID BeegoModel::freeWheelBallId(void) const
{
  return pimpl->beego_.free_wheel_ball.body;
}


void BeegoModel::setMotorVelocity(int id, int count_per_msec)
{
  pimpl->setMotorVelocity(id, count_per_msec);
}


long BeegoModel::encoderCount(int id) const
{
  long wheel_count = WheelEncoderCount[id];
  dReal radian = dJointGetHingeAngle(pimpl->beego_.drive_joint[id]);
  long count = static_cast<long>(wheel_count * radian / (2.0 * M_PI));
  long diff = pimpl->previous_encoder_count_[id] - count;

  long diff_abs = abs(diff);
  if (diff_abs > (wheel_count / 2)) {
    diff = (wheel_count - diff_abs - 1) * diff / diff_abs;
  }
  pimpl->previous_encoder_count_[id] = count;

  return diff;
}
