/*!
  \file
  \brief ビーゴの制御

  \author Satofumi KAMIMURA

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

#include "BeegoDrive.h"
#include "CommandPacket.h"
#include "SerialDevice.h"
#include "beego_drive.h"
#include "MathUtils.h"

using namespace qrk;
using namespace std;


struct BeegoDrive::pImpl
{
  string error_message_;
  Connection* connection_;
  bool serial_created_;
  CommandPacket command_packet_;


  pImpl(void)
    : error_message_("no error."),
      connection_(NULL), serial_created_(false)
  {
  }


  ~pImpl(void)
  {
    if (serial_created_) {
      delete connection_;
    }
  }


  bool connect(const char* device, long baudrate)
  {
    disconnect();
    if (! connection_) {
      setSerialDevice();
    }

    if (! connection_->connect(device, baudrate)) {
      error_message_ = connection_->what();
      return false;
    }

    unsigned char major;
    unsigned char minor;
    unsigned char micro;
    if (! command_packet_.version(&major, &minor, &micro)) {
      error_message_ = "no response from controller.";
      connection_->disconnect();
      return false;
    }

    if ((major != Major) || (minor != Minor)) {
      error_message_ = "please update controller version.";
      connection_->disconnect();
      return false;
    }

    // サーボ状態にする
    stop();

    return true;
  }


  void disconnect(void)
  {
    if (connection_) {
      connection_->disconnect();
    }
  }


  bool isConnected(void)
  {
    return (connection_ == NULL) ? false : connection_->isConnected();
  }


  void setSerialDevice(void)
  {
    // シリアル接続のオブジェクトを生成する
    setConnection(new SerialDevice);
    serial_created_ = true;
  }


  void setConnection(Connection* connection)
  {
    connection_ = connection;
    command_packet_.setConnection(connection);

    // SerialDevice のリソース管理はユーザ側に任せる
    serial_created_ = false;
  }


  void stop(void)
  {
    if (! command_packet_.stop()) {
      // !!! 例外を投げるべき
    }
  }


  long convertDir16(const Angle& angle)
  {
    return static_cast<long>(65536.0 * angle.to_rad() / (2.0 * M_PI));
  }
};


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


BeegoDrive::~BeegoDrive(void)
{
}


const char* BeegoDrive::what(void) const
{
  return pimpl->error_message_.c_str();
}


bool BeegoDrive::connect(const char* device, long baudrate)
{
  return pimpl->connect(device, baudrate);
}


void BeegoDrive::disconnect(void)
{
  pimpl->disconnect();
}


bool BeegoDrive::isConnected(void) const
{
  return pimpl->isConnected();
}


void BeegoDrive::setConnection(Connection* connection)
{
  pimpl->setConnection(connection);
}


Connection* BeegoDrive::connection(void)
{
  return pimpl->connection_;
}


void BeegoDrive::followLine(const Position<long>& line,
                            const Coordinate* coordinate)
{
  Position<long> target_position(line);
  if (coordinate) {
    target_position = coordinate->pointPosition(target_position, this);
  }

  long dir16 = pimpl->convertDir16(target_position.angle);
  if (! pimpl->command_packet_.followLine(target_position.x,
                                          target_position.y, dir16)) {
    // !!! 例外を投げるべき
  }
}


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


void BeegoDrive::stopLine(const Position<long>& line,
                          const Coordinate* coordinate)
{
  Position<long> target_position(line);
  if (coordinate) {
    target_position = coordinate->pointPosition(target_position, this);
  }

  long dir16 = pimpl->convertDir16(target_position.angle);
  if (! pimpl->command_packet_.stopLine(target_position.x,
                                        target_position.y, dir16)) {
    // !!! 例外を投げるべき
  }
}


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


void BeegoDrive::followCircle(const Point<long>& center, long radius,
                              const Coordinate* coordinate )
{
  Position<long> target_point(center.x, center.y, deg(0));
  if (coordinate) {
    target_point = coordinate->pointPosition(target_point, this);
  }

  if (! pimpl->command_packet_.followCircle(target_point.x,
                                            target_point.y, radius)) {
    // !!! 例外を投げるべき
  }
}


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


void BeegoDrive::stopCircle(const Point<long>& center, long radius,
                            const Angle& angle,
                            const Coordinate* coordinate)
{
  static_cast<void>(center);
  static_cast<void>(radius);
  static_cast<void>(angle);
  static_cast<void>(coordinate);

  // !!!
}


void BeegoDrive::spin(const Angle& angle_per_sec)
{
  long dir16 = pimpl->convertDir16(angle_per_sec);
  if (! pimpl->command_packet_.spin(dir16)) {
    // !!! 例外を投げるべき
  }
}


void BeegoDrive::rotate(const Angle& angle, const Coordinate* coordinate)
{
  Position<long> target_position(0, 0, angle);
  if (coordinate) {
    target_position = coordinate->pointPosition(target_position, this);
  }

  long dir16 = pimpl->convertDir16(target_position.angle);
  if (! pimpl->command_packet_.rotate(dir16)) {
    // !!! 例外を投げるべき
  }
}


void BeegoDrive::stop(void)
{
  pimpl->stop();
}


Position<long> BeegoDrive::position(Coordinate* coordinate) const
{
  Position<long> target_position;

  long mm[2] = { 0, 0 };
  unsigned short dir16 = 0;
  if (! pimpl->command_packet_.position(mm, &dir16)) {
    pimpl->error_message_ = "no response from controller.";
    pimpl->connection_->disconnect();

    // !!! 例外を投げるべき

    // (0, 0, deg(0)) のままデータを返す
    return target_position;
  }

  target_position.x = mm[0];
  target_position.y = mm[1];
  target_position.angle = rad(2.0 * M_PI * dir16 / 65536.0);

  if (coordinate) {
    return coordinate->pointPosition(target_position, this);
  } else {
    return target_position;
  }
}


long BeegoDrive::maxStraightVelocity(void) const
{
  long max_velocity = 0;
  if (! pimpl->command_packet_.maxStraightVelocity(&max_velocity)) {
    // !!! 例外を投げるべき
  }

  return max_velocity;
}


long BeegoDrive::straightVelocity(void) const
{
  long velocity = 0;
  if (! pimpl->command_packet_.straightVelocity(&velocity)) {
    // !!! 例外を投げるべき
  }

  return velocity;
}


Angle BeegoDrive::rotateVelocity(void) const
{
#if 0
  Angle velocity = 0;
  if (! pimpl->command_packet_.rotateVelocity(&velocity)) {
    // !!! 例外を投げるべき
  }

  return velocity;
#endif

  return deg(0);
}


void BeegoDrive::setRobotPosition(const Position<long>& position,
                                  const Coordinate* coordinate)
{
  static_cast<void>(position);
  static_cast<void>(coordinate);

  // !!!
}


void BeegoDrive::setStraightVelocity(long mm_per_sec)
{
  if (! pimpl->command_packet_.setStraightVelocity(mm_per_sec)) {
    // !!! 例外を投げるべき
  }
}


void BeegoDrive::setRotateVelocity(const Angle& angle_per_sec)
{
  static_cast<void>(angle_per_sec);

  // !!!
}


void BeegoDrive::setStraightAcceleration(long mm_per_sec2)
{
  static_cast<void>(mm_per_sec2);

  // !!!
}


void BeegoDrive::setRotateAcceleration(const Angle& angle_per_sec2)
{
  static_cast<void>(angle_per_sec2);

  // !!!
}


bool BeegoDrive::isStable(void) const
{
  bool stable = false;
  if (! pimpl->command_packet_.isStable(stable)) {
    // !!! 例外を投げるべき
  }

  return stable;
}


void BeegoDrive::setWheelVelocity(int id, long mm_per_sec)
{
  if (! pimpl->command_packet_.setWheelVelocity(id, mm_per_sec)) {
    // !!! 例外を投げるべき
  }
}


string BeegoDrive::currentCommand(void) const
{
  // !!!
  return "Not implemented.";
}


void BeegoDrive::saveCommand(void)
{
  // !!!
}


void BeegoDrive::restoreCommand(void)
{
  // !!!
}


void BeegoDrive::commitParameters(void)
{
  // !!!
}


#if 0
void BeegoDrive::setMotorParameter(int id, const motor_t& motor)
{
  static_cast<void>(id);
  static_cast<void>(motor);
  // !!!
}


void BeegoDrive::setEncoderParameter(int id, const encoder_t& encoder)
{
  static_cast<void>(id);
  static_cast<void>(encoder);
  // !!!
}


void BeegoDrive::setWheelParameter(int id, const wheel_t& wheel)
{
  static_cast<void>(id);
  static_cast<void>(wheel);
  // !!!
}


void BeegoDrive::setBodyParameter(const body_t& body)
{
  static_cast<void>(body);
  // !!!
}
#endif
