/*!
  \file
  \brief マーカーの管理

  \author Satofumi KAMIMURA

  $Id: MarkerManager.cpp 1440 2009-10-23 07:13:28Z satofumi $

  \todo 実機のみで動作させているときは、何も処理をしないようにする
*/

#include "MarkerManager.h"
#include "ExecutionType.h"
#include "OdeHandler.h"
#include "EventScheduler.h"
#include <map>
#include <string>

using namespace qrk;
using namespace std;


typedef pair<string, ModelManager::model_t*> ModelPair;
typedef multimap<string, ModelManager::model_t*>::iterator ModelsIterator;


struct MarkerManager::pImpl
{
  ExecutionType::Type type_;
  OdeHandler ode_;
  dWorldID world_;
  EventScheduler event_scheduler_;
  ModelManager model_manager_;
  multimap<string, ModelManager::model_t*> markers_;
  map<string, vector<dJointID> > marker_joints_;


  pImpl(void) : type_(ExecutionType::object()->type()), world_(ode_.world())
  {
  }


  static pImpl* object(void)
  {
    static pImpl singleton_object;
    return &singleton_object;
  }


  void drawPoint(const char* name, const Point<long>& point,
                 const Color& color, int width, long height)
  {
    ModelManager::model_t* model = new ModelManager::model_t;

    model->type = ModelManager::Box;
    model->body_id = dBodyCreate(world_);
    dBodySetPosition(model->body_id,
                     point.x / 1000.0, point.y / 1000.0, height / 1000.0);

    dJointID joint_id = dJointCreateFixed(world_, 0);
    dJointAttach(joint_id, model->body_id, 0);
    dJointSetFixed(joint_id);
    marker_joints_[name].push_back(joint_id);

    model->color = color;
    model->obstacle = false;
    dReal box_width[3];
    for (size_t i = 0; i < 3; ++i) {
      box_width[i] = width / 1000.0;
    }
    model_manager_.addBox(model, box_width);

    // multimap に model_t のデータを格納する
    markers_.insert(ModelPair(name, model));
  }


  void drawLine(const char* name, const Point<long>& begin,
                const Point<long>& end,
                const Color& color, int width, long height)
  {
    ModelManager::model_t* model = new ModelManager::model_t;

    model->type = ModelManager::Cylinder;
    model->body_id = dBodyCreate(world_);
    double length =
      sqrt(pow(end.x - begin.x, 2) + pow(end.y - begin.y, 2)) / 1000.0;
    double radian = atan2(end.y - begin.y, end.x - begin.x);
    dBodySetPosition(model->body_id,
                     (begin.x / 1000.0) + (length / 2.0 * cos(radian)),
                     (begin.y / 1000.0) + (length / 2.0 * sin(radian)),
                     height / 1000.0);
    dMatrix3 R;
    dRFromZAxis(R, end.x - begin.x, end.y - begin.y, 0.0);
    dBodySetRotation(model->body_id, R);

    dJointID joint_id = dJointCreateFixed(world_, 0);
    dJointAttach(joint_id, model->body_id, 0);
    dJointSetFixed(joint_id);
    marker_joints_[name].push_back(joint_id);

    model->color = color;
    model->obstacle = false;
    model_manager_.addCylinder(model, width / 1000.0 / 2.0, length);

    // multimap に model_t のデータを格納する
    markers_.insert(ModelPair(name, model));
  }
};


MarkerManager::MarkerManager(void) : pimpl(pImpl::object())
{
}


MarkerManager::~MarkerManager(void)
{
}


void MarkerManager::lock(void)
{
  if (pimpl->type_ != ExecutionType::RealDevice) {
    pimpl->event_scheduler_.lock();
  }
}


void MarkerManager::unlock(void)
{
  if (pimpl->type_ != ExecutionType::RealDevice) {
    pimpl->event_scheduler_.unlock();
  }
}


void MarkerManager::clear(void)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  while (! pimpl->markers_.empty()) {
    ModelsIterator it = pimpl->markers_.begin();
    clear(it->first.c_str());
  }
}


void MarkerManager::clear(const char* name)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  // ジョイントの破棄
  // !!! この付近でよく落ちる。要修正
  vector<dJointID>& joints = pimpl->marker_joints_[name];
  for (vector<dJointID>::iterator it = joints.begin();
       it != joints.end(); ++it) {
    dJointDestroy(*it);
  }
  pimpl->marker_joints_.erase(name);

  // モデルの破棄
  pair<ModelsIterator, ModelsIterator> p = pimpl->markers_.equal_range(name);
  for (ModelsIterator it = p.first; it != p.second; ++it) {
    ModelManager::model_t* model = it->second;
    pimpl->model_manager_.remove(model);
    dBodyDestroy(model->body_id);
    delete model;
  }
  pimpl->markers_.erase(p.first, p.second);
}


void MarkerManager::draw(const char* name, const ModelManager::model_t* model,
                         int life_msec)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  (void)name;
  (void)model;
  (void)life_msec;

  // !!!
}


void MarkerManager::drawModels(const char* name,
                               const ModelManager::model_t models[],
                               size_t n, int life_msec)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  (void)name;
  (void)models;
  (void)n;
  (void)life_msec;

  // !!!
}


void MarkerManager::drawPoint(const char* name, const Point<long>& point,
                              const Color& color, int radius, long height)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  pimpl->drawPoint(name, point, color, radius, height);
}


void MarkerManager::drawPoints(const char* name,
                               const std::vector<Point<long> >& points,
                               const Color& color, int radius, long height)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  for (vector<Point<long> >::const_iterator it = points.begin();
       it != points.end(); ++it) {
    pimpl->drawPoint(name, *it, color, radius, height);
  }
}


void MarkerManager::drawLines(const char* name,
                              const std::vector<Point<long> >& points,
                              const Color& color, int width,
                              int height)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  size_t n = points.size();
  if (n >= 2) {
    --n;
    for (size_t i = 0; i < n; ++i) {
      pimpl->drawLine(name, points[i], points[i + 1], color, width, height);
    }
  }
}


void MarkerManager::drawLinesStrip(const char* name,
                                   const std::vector<Point<long> >& points,
                                   const Color& color, int width,
                                   int life_msec)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  (void)name;
  (void)points;
  (void)color;
  (void)width;
  (void)life_msec;

  // !!!
}


void MarkerManager::drawArrows(const char* name,
                               const std::vector<Point<long> >& points,
                               const Color& color, int width,
                               int life_msec)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  (void)name;
  (void)points;
  (void)color;
  (void)width;
  (void)life_msec;

  // !!!
}


void MarkerManager::drawArrowsStrip(const char* name,
                                    const std::vector<Point<long> >& points,
                                    const Color& color, int width,
                                    int life_msec)
{
  if (pimpl->type_ == ExecutionType::RealDevice) {
    return;
  }

  (void)name;
  (void)points;
  (void)color;
  (void)width;
  (void)life_msec;

  // !!!
}
