/*!
  \file
  \brief BeegoDrive のシミュレータ

  \author Satofumi KAMIMURA

  $Id: BeegoDrive_device.cpp 1363 2009-10-03 08:12:07Z satofumi $
*/

#include "BeegoDrive_device.h"
#include "BeegoModel.h"
#include "DeviceManager.h"
#include "robot_control.h"
#include "packet_handler.h"
#include "serial_connection.h"
#include "log_printf.h"

using namespace qrk;
using namespace std;

extern void encoder_setModel(BeegoModel* model);
extern void motor_setModel(BeegoModel* model);


struct BeegoDrive_device::pImpl
{
  robot_t robot_;
  BeegoModel beego_model_;


  pImpl(const char* device)
  {
    DeviceManager device_manager;
    for (int i = 0; i < PortRetryMax; ++i) {
      long try_port = device_manager.nextPort();
      if (serial_setPort(try_port)) {
        device_manager.registerDevicePort(device, try_port);
        packet_initialize(&robot_);
        encoder_setModel(&beego_model_);
        motor_setModel(&beego_model_);
        return;
      }
    }

    log_printf("BeegoDrive_device(): fail assign port.\n");
    throw;
  }
};


BeegoDrive_device::BeegoDrive_device(const char* device)
  : pimpl(new pImpl(device))
{
}


BeegoDrive_device::~BeegoDrive_device(void)
{
}


void BeegoDrive_device::setParameter(const char* type,
                                            const char* parameter)
{
  (void)type;
  (void)parameter;

  // !!!
}


void BeegoDrive_device::activate(void)
{
  pimpl->beego_model_.activate();
  robot_initialize(&pimpl->robot_);
}


void BeegoDrive_device::execute(void)
{
  robot_update();
  packet_update();

  //Position<long> position = pimpl->beego_model_.position();
  //fprintf(stderr, "(%d, %d, %d)\n", position.x, position.y, position.deg());
}


size_t BeegoDrive_device::nextExecuteInterval(void) const
{
  return 1;
}


void BeegoDrive_device::setPosition(const qrk::Position<long>& position,
                                           OdeModel* model, bool fixed)
{
  pimpl->beego_model_.setPosition(position, model, fixed);
}


Position<long> BeegoDrive_device::position(void)
{
  return pimpl->beego_model_.position();
}


void BeegoDrive_device::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 BeegoDrive_device::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;

  // !!!
}


dBodyID BeegoDrive_device::objectId(void) const
{
  return pimpl->beego_model_.objectId();
}
