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

  \author Satofumi KAMIMURA

  $Id: Cylinder.cpp 1271 2009-09-01 13:53:04Z satofumi $

  \todo enum, object_t を共通のヘッダで宣言する
*/

#include "Cylinder.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 Cylinder::pImpl
{
  OdeHandler ode_;
  dWorldID world_;
  dSpaceID space_;
  dGeomID ground_;

  ModelManager model_manager_;
  ModelManager::model_t cylinder_model_;
  Color color_;

  dReal radius_;
  dReal length_;
  dReal mass_;
  object_t cylinder_;


  pImpl(long radius, long length, dReal mass)
    : world_(ode_.world()), space_(ode_.space()), ground_(ode_.ground()),
      color_(1.0 * 0x0d / 0xff, 1.0 * 0xba / 0xff, 1.0 * 0x64 / 0xff),
      radius_(radius / 1000.0), length_(length / 1000.0), mass_(mass)
  {
    createModel();
  }


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


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

    dMass mass;
    dMassSetZero(&mass);
    dMassSetCylinderTotal(&mass, mass_, 2, radius_, length_);
    dBodySetMass(cylinder_.body, &mass);
    cylinder_.geometry = dCreateCylinder(space_, radius_, length_);
    dGeomSetBody(cylinder_.geometry, cylinder_.body);
    dBodySetPosition(cylinder_.body, 0.0, 0.0, length_ / 2.0);

    registerModel();
  }


  void registerModel(void)
  {
    ModelManager model_manager;

    cylinder_model_.type = ModelManager::Cylinder;
    cylinder_model_.body_id = cylinder_.body;
    cylinder_model_.color = color_;
    cylinder_model_.obstacle = true;
    model_manager.addCylinder(&cylinder_model_, radius_, length_);

  }
};


Cylinder::Cylinder(long radius, long length, dReal mass)
  : pimpl(new pImpl(radius, length, mass))
{
}


Cylinder::~Cylinder(void)
{
}


void Cylinder::setPosition(const Position<long>& position,
                           OdeModel* model, bool fixed)
{
  dReal x = position.x / 1000.0;
  dReal y = position.y / 1000.0;
  dReal z = pimpl->length_ / 2.0;
  dGeomSetPosition(pimpl->cylinder_.geometry, x, y, z);

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


Position<long> Cylinder::position(void)
{
  const dReal* position = dBodyGetPosition(pimpl->cylinder_.body);

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


void Cylinder::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 Cylinder::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 Cylinder::setColor(const Color& color)
{
  pimpl->color_ = color;
}


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