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

  \author Satofumi KAMIMURA

  $Id: mBeegoDrive.cpp 1383 2009-10-09 03:20:36Z satofumi $
*/

#include "mBeegoDrive.h"
#include "BeegoDrive.h"
#include "DeviceManager.h"
#include "ExecutionType.h"
#include "TcpipSocket.h"

using namespace qrk;
using namespace std;


struct mBeegoDrive::pImpl
{
  BeegoDrive run_;
  ExecutionType::Type type_;
  Connection* created_connection_;


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


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


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


mBeegoDrive::~mBeegoDrive(void)
{
}


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


bool mBeegoDrive::connect(const char* device, long baudrate)
{
  // !!! 発行コマンドをログに表示する

  DeviceManager device_manager;
  long port;

  switch (pimpl->type_) {
  case ExecutionType::Simulation:
    // ポート番号を取得し、TCP/IP として接続する
    port = device_manager.port(device);
    return pimpl->run_.connect("localhost", port);
    break;

  case ExecutionType::RealDevice:
  case ExecutionType::Monitoring:
  case ExecutionType::Recording:
    // 指定されたデバイスにシリアルとして接続する
    return pimpl->run_.connect(device, baudrate);
    break;

  case ExecutionType::Playback:
    // !!! 接続しない
    // !!! 記録しておいた結果を読み出して返す
    // !!!
    return false;
    break;
  }

  return false;
}


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

  pimpl->run_.disconnect();
}


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

  return pimpl->run_.isConnected();
}


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();
}


Position<long> mBeegoDrive::position(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::setRobotPosition(const Position<long>& position,
                                   const Coordinate* coordinate)
{
  // !!! 発行コマンドをログに表示する

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


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
