/*!
  \file
  \brief モニタ対応の走行制御クラス

  \author Satofumi KAMIMURA

  $Id: mDifferentialDrive.cpp 439 2009-01-03 05:01:55Z satofumi $
*/

#include "mDifferentialDrive.h"
#include "mConnection.h"
#include "SerialCtrl.h"
#include "TcpipCtrl.h"
#include "MonitorDataHandler.h"
#include "LogNameHolder.h"
#include "DeviceIpManager.h"

using namespace qrk;


struct mDifferentialDrive::pImpl
{
  MonitorModeManager::MonitorMode mode_;
  DifferentialDrive run_;
  Connection* monitor_con_;
  Connection* device_con_;
  MonitorDataHandler* handler_;


  pImpl(int argc, char *argv[])
    : monitor_con_(NULL), device_con_(NULL), handler_(NULL)
  {
    MonitorModeManager::object()->setMode(argc, argv);
    mode_ = MonitorModeManager::object()->mode();

    // mConnection 用のログ管理
    if (mode_ != MonitorModeManager::Simulation) {
      device_con_ = new SerialCtrl;
    } else {
      device_con_ = new TcpipCtrl;
    }
    monitor_con_ = new mConnection(device_con_);
    run_.setConnection(monitor_con_);

    // mDifferentialDrive 用のログ管理
    std::string log_name =
      LogNameHolder::object()->name("mDifferentialDrive");
    handler_ = new MonitorDataHandler(log_name, mode_);
  }


  ~pImpl(void)
  {
    if (monitor_con_) {
      delete monitor_con_;
    }
    if (device_con_) {
      delete device_con_;
    }
    delete handler_;
  }
};


mDifferentialDrive::mDifferentialDrive(int argc, char *argv[])
  : pimpl(new pImpl(argc, argv))
{
}


mDifferentialDrive::~mDifferentialDrive(void)
{
}


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


void mDifferentialDrive::pushState(void)
{
  pimpl->run_.pushState();
}


void mDifferentialDrive::popState(void)
{
  pimpl->run_.popState();
}


bool mDifferentialDrive::connect(const char* device, long baudrate)
{
  MonitorModeManager::MonitorMode Simulation = MonitorModeManager::Simulation;
  if (MonitorModeManager::object()->mode() == Simulation) {
    // device 名で IP ポート取得し、サーバを起動する
    long port = DeviceIpManager::object()->createIpPort(device);
    if (port > 0) {
      // IP ポートが取得済みでない場合のみ、サーバを起動する
      // !!! URG の型、シリアル ID の情報を、引数で反映させるべき
#if 0
      UrgServer* urg_server = new UrgServer;
      if (! urg_server->activate(port)) {
        // サーバが起動できなければ、接続を中断する
        return false;
      }
      MonitorEventScheduler::singleton()->registerDeviceServer(urg_server);
#endif
    }
  }
  return pimpl->run_.connect(device, baudrate);
}

#if 0
bool mDifferentialDrive::connect(Connection* con)
{
  return pimpl->run_.connect(con);
}
#endif


void mDifferentialDrive::setConnection(Connection* con)
{
  pimpl->run_.setConnection(con);
}


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


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


bool mDifferentialDrive::isConnected(void)
{
  return pimpl->run_.isConnected();
}


bool mDifferentialDrive::setTimestamp(int ticks,
                                      int* response_msec,
                                      int* force_delay_msec)
{
  if ((pimpl->mode_ == MonitorModeManager::Record) ||
      (pimpl->mode_ == MonitorModeManager::Play)) {
    pimpl->handler_->fetch(ticks);
  }

  int recorded = (force_delay_msec) ? *force_delay_msec : 0;
  if (pimpl->mode_ == MonitorModeManager::Play) {
    pimpl->handler_->fetch(recorded);
  }

  int delay_msec = 0;
  bool ret = pimpl->run_.setTimestamp(ticks, &delay_msec, &recorded);
  if (response_msec) {
    *response_msec = delay_msec;
  }
  if (pimpl->mode_ == MonitorModeManager::Record) {
    pimpl->handler_->fetch(delay_msec);
  }
  return ret;
}


void mDifferentialDrive::setServo(bool on)
{
  pimpl->run_.setServo(on);
}


void mDifferentialDrive::followLine(const Position<long>& position)
{
  pimpl->run_.followLine(position);
}


void mDifferentialDrive::followCircle(const Grid<long>& point, const int radius)
{
  pimpl->run_.followCircle(point, radius);
}


void mDifferentialDrive::stopToLine(const Grid<long>& point, const Angle& angle)
{
  pimpl->run_.stopToLine(point, angle);
}


void mDifferentialDrive::stopToDirection(const Angle& angle)
{
  pimpl->run_.stopToDirection(angle);
}


void mDifferentialDrive::rotateAngle(const Angle& angle)
{
  pimpl->run_.rotateAngle(angle);
}


void mDifferentialDrive::rotate(const Angle& angle_sec)
{
  pimpl->run_.rotate(angle_sec);
}


void mDifferentialDrive::stop(void)
{
  pimpl->run_.stop();
}


Position<long> mDifferentialDrive::position(Coordinate* coordinate,
                                            int* timestamp)
{
  return pimpl->run_.position(coordinate, timestamp);
}


void mDifferentialDrive::setRobotPosition(const Position<long>& position)
{
  return pimpl->run_.setRobotPosition(position);
}


bool mDifferentialDrive::isStable(void)
{
  return pimpl->run_.isStable();
}


void mDifferentialDrive::setStraightVelocity(int mm_sec)
{
  pimpl->run_.setStraightVelocity(mm_sec);
}


int mDifferentialDrive::straightVelocity(void)
{
  return pimpl->run_.straightVelocity();
}


void mDifferentialDrive::setStraightAcceleration(int mm_sec2)
{
  pimpl->run_.setStraightAcceleration(mm_sec2);
}


void mDifferentialDrive::setRotateVelocity(const Angle& angle_sec)
{
  pimpl->run_.setRotateVelocity(angle_sec);
}


Angle mDifferentialDrive::rotateVelocity(void)
{
  return pimpl->run_.rotateVelocity();
}


void mDifferentialDrive::setRotateAcceleration(const Angle& angle_sec2)
{
  pimpl->run_.setRotateAcceleration(angle_sec2);
}


void mDifferentialDrive::setWheelVel(int id, int mm_vel)
{
  pimpl->run_.setWheelVel(id, mm_vel);
}
