/*!
  \file
  \brief URG の ODE 用モデル

  \author Satofumi KAMIMURA

  $Id: UrgModel.cpp 1584 2009-12-25 23:25:11Z satofumi $
*/

#include "UrgModel.h"
#include "EventScheduler.h"
#include "OdeModel.h"
#include "OdeHandler.h"
#include "ModelManager.h"

using namespace qrk;


namespace
{
  enum {
    X = 0,
    Y = 1,
    Z = 2,
  };


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


  typedef struct
  {
    object_t top;
    object_t base;
    dJointID fixed_joint;
  } sensor_t;
}


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

  enum {
    TopModel = 0,
    BaseModel,
  };
  ModelManager::model_t models_[2];

  EventScheduler event_manager_;

  sensor_t sensor_;
  SensorType sensor_type_;
  dReal sensor_length_[3];
  dReal sensor_weight_;
  dReal slit_;

  bool fixed_;
  dJointID fixed_joint_;


  pImpl(void)
    : world_(ode_.world()), space_(ode_.space()), sensor_type_(UTM_30LX),
      slit_(0.010), fixed_(false)
  {
  }


  void createModel(void)
  {
    if (sensor_type_ == URG_04LX) {
      sensor_length_[X] = 0.050;
      sensor_length_[Y] = 0.050;
      sensor_length_[Z] = 0.055;
      sensor_weight_ = 0.300;

    } else {
      // UTM_30LX
      sensor_length_[X] = 0.060;
      sensor_length_[Y] = 0.060;
      sensor_length_[Z] = 0.055;
      sensor_weight_ = 0.300;
    }

    // センサ上部
    sensor_.top.body = dBodyCreate(world_);

    dMass mass;
    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass, sensor_weight_ / 2.0,
                     sensor_length_[X], sensor_length_[Y],
                     (sensor_length_[Z] - slit_) / 2.0);
    dBodySetMass(sensor_.top.body, &mass);
    sensor_.top.geometry =
      dCreateBox(space_, sensor_length_[X], sensor_length_[Y],
                 (sensor_length_[Z] - slit_) / 2.0);
    dGeomSetBody(sensor_.top.geometry, sensor_.top.body);
    dBodySetPosition(sensor_.top.body, 0.0, 0.0,
                     sensor_length_[Z] + slit_ + 0.001);

    // センサ下部
    sensor_.base.body = dBodyCreate(world_);

    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass, sensor_weight_ / 2.0,
                     sensor_length_[X], sensor_length_[Y],
                     (sensor_length_[Z] - slit_) / 2.0);
    dBodySetMass(sensor_.base.body, &mass);
    sensor_.base.geometry =
      dCreateBox(space_,
                 sensor_length_[X], sensor_length_[Y],
                 (sensor_length_[Z] - slit_) / 2.0);
    dGeomSetBody(sensor_.base.geometry, sensor_.base.body);
    dBodySetPosition(sensor_.base.body, 0.0, 0.0,
                     (sensor_length_[Z] / 2.0) + 0.001);

    // ジョイント
    sensor_.fixed_joint = dJointCreateFixed(world_, 0);
    dJointAttach(sensor_.fixed_joint,
                 sensor_.base.body, sensor_.top.body);
    dJointSetFixed(sensor_.fixed_joint);

    registerModel();
  }


  void registerModel(void)
  {
    Color color(0.3, 0.3, 0.3);

    ModelManager model_manager;

    // センサ上部
    models_[TopModel].type = ModelManager::Box;
    models_[TopModel].body_id = sensor_.top.body;
    models_[TopModel].color = color;
    models_[TopModel].obstacle = false;
    model_manager.addBox(&models_[TopModel], sensor_length_);

    // センサ下部
    models_[BaseModel].type = ModelManager::Box;
    models_[BaseModel].body_id = sensor_.base.body;
    models_[BaseModel].color = color;
    models_[BaseModel].obstacle = false;
    model_manager.addBox(&models_[BaseModel], sensor_length_);
  }
};


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


UrgModel::~UrgModel(void)
{
}


void UrgModel::setSensorType(SensorType type)
{
  pimpl->sensor_type_ = type;
}


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


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

  dReal x = position.x / 1000.0;
  dReal y = position.y / 1000.0;
  dReal z = (pimpl->sensor_length_[Z] / 2.0) + 0.001;

  // 指定モデルの相対位置に配置する
  dBodyID base_body_id = 0;
  if (model) {
    base_body_id = model->objectId();
    const dReal* base_position = dBodyGetPosition(base_body_id);
    x += base_position[X];
    y += base_position[Y];
    z += base_position[Z];

    // !!! 角度にも相対値を加算する
  }

  // !!! 傾いているときにも、配置位置が適切になるようにする
  dGeomSetPosition(pimpl->sensor_.top.geometry, x, y,
                   z + pimpl->sensor_length_[Z]);
  dGeomSetPosition(pimpl->sensor_.base.geometry, x, y, z);

  if (fixed) {
    if (pimpl->fixed_) {
      dJointDestroy(pimpl->fixed_joint_);
    }
    pimpl->fixed_joint_ = dJointCreateFixed(pimpl->world_, 0);
    dJointAttach(pimpl->fixed_joint_,
                 pimpl->sensor_.base.body, base_body_id);
    dJointSetFixed(pimpl->fixed_joint_);
    pimpl->fixed_ = true;
  }
}


Position<long> UrgModel::position(void) const
{
  long px;
  long py;
  long pz;
  double dx;
  double dy;
  double dz;

  position3d(px, py, pz, dx, dy, dz);

  return Position<long>(px, py, rad(atan2(dy, dx)));
}


void UrgModel::setPosition3d(const Point3d<long>& position,
                             double ax, double ay, double az,
                             const Angle& angle,
                             OdeModel* model, bool fixed)
{
  // !!! 2D の設定とくくる

  dReal x = position.x / 1000.0;
  dReal y = position.y / 1000.0;
  dReal z = (position.z / 1000.0) + (pimpl->sensor_length_[Z] / 2.0);

  // 指定モデルの相対位置に配置する
  Angle adjusted_angle = angle;
  dBodyID base_body_id = 0;
  if (model) {
    base_body_id = model->objectId();
    const dReal* base_position = dBodyGetPosition(base_body_id);
    x += base_position[X];
    y += base_position[Y];
    z += base_position[Z];

    // !!! 角度計算を理解したら、適切な実装に修正する
    const dReal* base_rotation = dBodyGetRotation(base_body_id);
    adjusted_angle
      += rad(atan2(-base_rotation[0], base_rotation[4]) + (M_PI / 2.0));
  }

  // 指定角度に回転
  dMatrix3 R;
  dRFromAxisAndAngle(R, ax, ay, az, adjusted_angle.to_rad());

  dGeomSetRotation(pimpl->sensor_.top.geometry, R);
  dGeomSetRotation(pimpl->sensor_.base.geometry, R);

  // !!! 傾いているときにも、配置位置が適切になるようにする
  dGeomSetPosition(pimpl->sensor_.top.geometry,
                   x, y, z + pimpl->sensor_length_[Z]);
  dGeomSetPosition(pimpl->sensor_.base.geometry, x, y, z);

  if (fixed) {
    if (pimpl->fixed_) {
      dJointDestroy(pimpl->fixed_joint_);
    }
    pimpl->fixed_joint_ = dJointCreateFixed(pimpl->world_, 0);
    dJointAttach(pimpl->fixed_joint_,
                 pimpl->sensor_.base.body, base_body_id);
    dJointSetFixed(pimpl->fixed_joint_);
    pimpl->fixed_ = true;
  }
}


void UrgModel::position3d(long& px, long& py, long& pz,
                          double& dx, double& dy, double& dz) const
{
  const dReal* position = dBodyGetPosition(pimpl->sensor_.base.body);
  const dReal* rotation = dBodyGetRotation(pimpl->sensor_.base.body);

  px = static_cast<long>(1000.0 * position[X]);
  py = static_cast<long>(1000.0 * position[Y]);
  pz = static_cast<long>(1000.0 * position[Z]);

  // !!! 適切な向きを返せるようにする
  (void)rotation;
  dx = 1.0;
  dy = 0.0;
  dz = 0.0;
}


dBodyID UrgModel::objectId(void) const
{
  return pimpl->sensor_.base.body;
}


void UrgModel::setLaser(long* buffer, double radian, long max_length)
{
  dGeomID ray_id = dCreateRay(pimpl->space_, max_length / 1000.0);

  // URG 位置を基準として、指定された向きにレーザーを配置する
  const dReal* rotation = dBodyGetRotation(pimpl->sensor_.base.body);

  // スキャニング平面の基底
  double e1[3];
  e1[0] = rotation[0];
  e1[1] = rotation[4];
  e1[2] = rotation[8];

  double e2[3];
  e2[0] = rotation[1];
  e2[1] = rotation[5];
  e2[2] = rotation[9];

  // レーザの向きを計算
  double laser[3];
  for (size_t i = 0; i < 3; ++i) {
    laser[i] = (e1[i] * cos(radian)) + (e2[i] * sin(radian));
  }

  // センサ基準で配置したレーザを回転させる
  // センサの始点は、上部と下部の中点とする
  const dReal* top_position = dBodyGetPosition(pimpl->sensor_.top.body);
  const dReal* base_position = dBodyGetPosition(pimpl->sensor_.base.body);
  dGeomRaySet(ray_id,
              (base_position[X] + top_position[X]) / 2.0,
              (base_position[Y] + top_position[Y]) / 2.0,
              (base_position[Z] + top_position[Z]) / 2.0,
              laser[X], laser[Y], laser[Z]);
  pimpl->event_manager_.setLaser(buffer, ray_id);
}
