/*!
  \file
  \brief シミュレーション用の障害物(Box)

  \author Satofumi KAMIMURA

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

#include "Box.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;
}


struct Box::pImpl
{
  OdeHandler ode_;
  dWorldID world_;
  dSpaceID space_;
  dGeomID ground_;

  ModelManager model_manager_;
  ModelManager::model_t box_model_;
  Color color_;

  dReal length_[3];
  dReal mass_;
  object_t box_;


  pImpl(long x, long y, long z, dReal mass)
    : world_(ode_.world()), space_(ode_.space()), ground_(ode_.ground()),
      color_(1.0 * 0x0d / 0xff, 1.0 * 0xba / 0xff, 1.0 * 0x64 / 0xff),
      mass_(mass)
  {
    length_[X] = x / 1000.0;
    length_[Y] = y / 1000.0;
    length_[Z] = z / 1000.0;

    createModel();
  }


  ~pImpl(void)
  {
    dBodyDestroy(box_.body);
  }


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

    dMass mass;
    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass, mass_, length_[X], length_[Y], length_[Z]);
    dBodySetMass(box_.body, &mass);
    box_.geometry = dCreateBox(space_, length_[X], length_[Y], length_[Z]);
    dGeomSetBody(box_.geometry, box_.body);
    dBodySetPosition(box_.body, 0.0, 0.0, length_[Z] / 2.0);

    registerModel();
  }


  void registerModel(void)
  {
    ModelManager model_manager;

    box_model_.type = ModelManager::Box;
    box_model_.body_id = box_.body;
    box_model_.color = color_;
    box_model_.obstacle = true;
    model_manager.addBox(&box_model_, length_);
  }
};


Box::Box(long x, long y, long z, dReal mass) : pimpl(new pImpl(x, y, z, mass))
{
}


Box::~Box(void)
{
}


void Box::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->box_.geometry, R);

  dReal x = position.x / 1000.0;
  dReal y = position.y / 1000.0;
  dReal z = pimpl->length_[Z] / 2.0;
  dGeomSetPosition(pimpl->box_.geometry, x, y, z);

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


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

  return Position<long>(static_cast<long>(1000.0 * position[X]),
                        static_cast<long>(1000.0 * position[Y]), rad(radian));
}


void Box::setPosition3d(const Point3d<long>& position,
                        double ax, double ay, double az,
                        const Angle& angle,
                        OdeModel* model, bool fixed)
{
  (void)position;
  (void)ax;
  (void)ay;
  (void)az;
  (void)angle;

  (void)model;
  (void)fixed;

  // !!!
}


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

  // !!!
}


void Box::setColor(const Color& color)
{
  pimpl->color_ = color;
}


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