/*!
  \file
  \brief モニタ対応の BeegoDrive クラス

  \author Satofumi KAMIMURA

  $Id: mBeegoDrive.cpp 1583 2009-12-20 02:12:07Z satofumi $
*/

#include "mBeegoDrive.h"
#include "BeegoDrive.h"
#include "DeviceManager.h"
#include "MonitorDataHandler.h"
#include "LogNameHolder.h"
#include "mConnection.h"
#include "SerialDevice.h"
#include "TcpipSocket.h"

using namespace qrk;
using namespace std;


struct mBeegoDrive::pImpl
{
  BeegoDrive run_;
  ExecutionType::Type type_;
  SerialDevice* serial_;
  Connection* created_connection_;
  MonitorDataHandler* handler_;


  pImpl(void)
    : type_(ExecutionType::object()->type()),
      serial_(NULL), created_connection_(NULL), handler_(NULL)
  {
    if (type_ == ExecutionType::Simulation) {
      created_connection_ = new TcpipSocket;
      run_.setConnection(created_connection_);

    } else {
      serial_= new SerialDevice;

      if ((type_ == ExecutionType::Recording) ||
          (type_ == ExecutionType::Playback)) {
        string log_name = LogNameHolder::object()->name("mBeegoDrive");
        handler_ = new MonitorDataHandler(log_name, type_);
        created_connection_ = new mConnection(serial_);
        run_.setConnection(created_connection_);

      } else {
        run_.setConnection(serial_);
      }
    }
  }


  ~pImpl(void)
  {
    delete handler_;
    delete created_connection_;
    delete serial_;
  }
};


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


mBeegoDrive::~mBeegoDrive(void)
{
}


const char* mBeegoDrive::what(void) const
{
  return pimpl->run_.what();
}


long mBeegoDrive::timestamp(void) const
{
  if (pimpl->type_ == ExecutionType::Simulation) {
    return pimpl->run_.timestamp();

  } else {
    long ret = pimpl->run_.timestamp();
    if ((pimpl->type_ == ExecutionType::Recording) ||
        (pimpl->type_ == ExecutionType::Playback)) {
      pimpl->handler_->fetch(ret);
    }
    return ret;
  }
}


bool mBeegoDrive::connect(const char* device, long baudrate)
{
  if (pimpl->type_ == ExecutionType::Simulation) {
    // ポート番号を取得し、TCP/IP として接続する
    DeviceManager device_manager;
    long port = device_manager.port(device);
    return pimpl->run_.connect("localhost", port);

  } else {
    bool ret = pimpl->run_.connect(device, baudrate);
    if ((pimpl->type_ == ExecutionType::Recording) ||
        (pimpl->type_ == ExecutionType::Playback)) {
      if (pimpl->handler_->fetch(ret) < 0) {
        disconnect();
        return false;
      }
    }
    return ret;
  }
}


void mBeegoDrive::disconnect(void)
{
  pimpl->run_.disconnect();
}


bool mBeegoDrive::isConnected(void) const
{
  // 戻り値のみを記録する
  bool ret = false;
  if (pimpl->type_ != ExecutionType::Playback) {
    ret = pimpl->run_.isConnected();
  }

  if ((pimpl->type_ == ExecutionType::Recording) ||
      (pimpl->type_ == ExecutionType::Playback)) {
    pimpl->handler_->fetch(ret);
  }
  return ret;
}


void mBeegoDrive::setConnection(Connection* connection)
{
  pimpl->run_.setConnection(connection);
}


Connection* mBeegoDrive::connection(void)
{
  return pimpl->run_.connection();
}


void mBeegoDrive::followLine(const Position<long>& line,
                             const Coordinate* coordinate)
{
  // !!! 発行コマンドをログに表示する
  // !!! 発行コマンドを画面に描画する

  pimpl->run_.followLine(line, coordinate);
}


void mBeegoDrive::followLine(long x, long y, const Angle& angle,
                             const Coordinate* coordinate)
{
  followLine(Position<long>(x, y, angle), coordinate);
}


void mBeegoDrive::stopLine(const Position<long>& line,
                           const Coordinate* coordinate)
{
  // !!! 発行コマンドをログに表示する
  // !!! 発行コマンドを画面に描画する

  pimpl->run_.stopLine(line, coordinate);
}


void mBeegoDrive::stopLine(long x, long y, const Angle& angle,
                           const Coordinate* coordinate)
{
  stopLine(Position<long>(x, y, angle), coordinate);
}


void mBeegoDrive::followCircle(const Point<long>& center, long radius,
                               const Coordinate* coordinate)
{
  // !!! 発行コマンドをログに表示する
  // !!! 発行コマンドを画面に描画する

  pimpl->run_.followCircle(center, radius, coordinate);
}


void mBeegoDrive::followCircle(long x, long y, long radius,
                               const Coordinate* coordinate )
{
  followCircle(Point<long>(x, y), radius, coordinate);
}


void mBeegoDrive::stopCircle(const Point<long>& center,
                             long radius, const Angle& angle,
                             const Coordinate* coordinate)
{
  // !!! 発行コマンドをログに表示する
  // !!! 発行コマンドを画面に描画する

  pimpl->run_.stopCircle(center, radius, angle, coordinate);
}


void mBeegoDrive::spin(const Angle& angle)
{
  // !!! 発行コマンドをログに表示する
  // !!! 発行コマンドを画面に描画する

  pimpl->run_.spin(angle);
}


void mBeegoDrive::rotate(const Angle& angle,
                         const Coordinate* coordinate)
{
  // !!! 発行コマンドをログに表示する
  // !!! 発行コマンドを画面に描画する

  pimpl->run_.rotate(angle, coordinate);
}


void mBeegoDrive::stop(void)
{
  // !!! 発行コマンドをログに表示する
  // !!! 発行コマンドを画面に描画する

  pimpl->run_.stop();
}


void mBeegoDrive::freeServo(void)
{
  pimpl->run_.freeServo();
}


Position<long> mBeegoDrive::position(const Coordinate* coordinate) const
{
  // !!! 発行コマンドをログに表示する

  return pimpl->run_.position(coordinate);
}


long mBeegoDrive::maxStraightVelocity(void) const
{
  // !!! 発行コマンドをログに表示する

  return pimpl->run_.maxStraightVelocity();
}


long mBeegoDrive::straightVelocity(void) const
{
  // !!! 発行コマンドをログに表示する

  return pimpl->run_.straightVelocity();
}


Angle mBeegoDrive::rotateVelocity(void) const
{
  // !!! 発行コマンドをログに表示する

  return pimpl->run_.rotateVelocity();
}


void mBeegoDrive::changeRobotPosition(const Position<long>& to,
                                      const Position<long>& from)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.changeRobotPosition(to, from);
}


#if 0
void mBeegoDrive::setRobotPosition(const Position<long>& position,
                                   const Coordinate* coordinate)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setRobotPosition(position, coordinate);
}


void mBeegoDrive::setRobotPositionOffset(const Position<long>& offset)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setRobotPositionOffset(offset);
}
#endif


void mBeegoDrive::setStraightVelocity(long mm_per_sec)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setStraightVelocity(mm_per_sec);
}


void mBeegoDrive::setRotateVelocity(const Angle& angle_per_sec)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setRotateVelocity(angle_per_sec);
}


void mBeegoDrive::setStraightAcceleration(long mm_per_sec2)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setStraightAcceleration(mm_per_sec2);
}


void mBeegoDrive::setRotateAcceleration(const Angle& angle_per_sec2)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setRotateAcceleration(angle_per_sec2);
}


bool mBeegoDrive::isStable(void) const
{
  // !!! 発行コマンドをログに表示する

  return pimpl->run_.isStable();
}


void mBeegoDrive::setWheelVelocity(int id, long mm_per_sec)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setWheelVelocity(id, mm_per_sec);
}


string mBeegoDrive::currentCommand(void) const
{
  // !!! 発行コマンドをログに表示する

  return pimpl->run_.currentCommand();
}


void mBeegoDrive::saveCommand(void)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.saveCommand();
}


void mBeegoDrive::restoreCommand(void)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.restoreCommand();
}


void mBeegoDrive::commitParameters(void)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.commitParameters();
}


#if 0
void mBeegoDrive::setMotorParameter(int id, const motor_t& motor)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setMotorParameter(id, motor);
}


void mBeegoDrive::setEncoderParameter(int id, const encoder_t& encoder)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setEncoderParameter(id, encoder);
}


void mBeegoDrive::setWheelParameter(int id, const wheel_t& wheel)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setWheelParameter(id, wheel);
}


void mBeegoDrive::setBodyParameter(const body_t& body)
{
  // !!! 発行コマンドをログに表示する

  pimpl->run_.setBodyParameter(body);
}
#endif
