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

  \author Satofumi KAMIMURA

  $Id: BeegoDrive.cpp 1601 2010-01-04 15:09:50Z satofumi $
*/

//#define USING_RUN_CTRL

#include "BeegoDrive.h"
#include "CommandPacket.h"
#include "SerialDevice.h"
#include "Rotate.h"
#if defined(USING_RUN_CTRL)
#include "runCtrl.h"
#endif
#include "beego_drive.h"
#include <boost/numeric/ublas/matrix.hpp>

#if defined(USING_RUN_CTRL)
using namespace VXV;
#endif

using namespace qrk;
using namespace boost;
using namespace boost::numeric;
using namespace boost::numeric::ublas;
using namespace std;


struct BeegoDrive::pImpl
{
  string error_message_;
  Connection* connection_;
  bool serial_created_;
#if defined(USING_RUN_CTRL)
  RunCtrl run_;
#else
  CommandPacket command_packet_;
#endif
  matrix<double> offset_;
  matrix<double> offset_invert_;
  Angle angle_offset_;


  pImpl(void)
    : error_message_("no error."), connection_(NULL), serial_created_(false),
      offset_(4, 4), offset_invert_(4, 4)
  {
    // 単位行列として初期化する
    for (int y = 0; y < 4; ++y) {
      for (int x = 0; x < 4; ++x) {
        offset_(y, x) = (y == x) ? 1.0 : 0.0;
        offset_invert_(y, x) = (y == x) ? 1.0 : 0.0;
      }
    }
  }


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


  bool connect(const char* device, long baudrate)
  {
    disconnect();
    if (! connection_) {
      setSerialDevice();
    }
#if defined(USING_RUN_CTRL)
    if (run_.connect(device, baudrate) < 0) {
      error_message_ = run_.what();
      return false;
    }
    return true;

#else

    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;
    }
#endif

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

    return true;
  }


  void disconnect(void)
  {
#if defined(USING_RUN_CTRL)
    run_.disconnect();
#else
    if (connection_) {
      connection_->disconnect();
    }
#endif
  }


  bool isConnected(void)
  {
#if defined(USING_RUN_CTRL)
    return run_.isConnected();
#else
    return (connection_ == NULL) ? false : connection_->isConnected();
#endif
  }


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


  void setConnection(Connection* connection)
  {
#if defined(USING_RUN_CTRL)
    connection_ = connection;
    run_.setConnection(connection);
#else

    connection_ = connection;
    command_packet_.setConnection(connection);

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


  void stop(void)
  {
#if defined(USING_RUN_CTRL)
    run_.stop();
#else
    if (! command_packet_.stop()) {
      // !!! 例外を投げるべき
    }
#endif
  }


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


  // 向きの補正と位置の補正を適用する
  Position<long> adjustPosition(const Position<long>& original,
                                bool invert = false)
  {
    matrix<double> adjusted;
    if (! invert) {
      matrix<double> original_matrix(1, 4);
      original_matrix(0, 0) = original.x;
      original_matrix(0, 1) = original.y;
      original_matrix(0, 2) = 0.0;
      original_matrix(0, 3) = 1.0;

      adjusted = prod(original_matrix, offset_);
      return Position<long>(adjusted(0, 0),
                            adjusted(0, 1),
                            original.angle + angle_offset_);

    } else {
      matrix<double> original_matrix(4, 1);
      original_matrix(0, 0) = original.x;
      original_matrix(1, 0) = original.y;
      original_matrix(2, 0) = 0.0;
      original_matrix(3, 0) = 1.0;

      adjusted = prod(offset_invert_, original_matrix);
      return Position<long>(adjusted(0, 0),
                            adjusted(1, 0),
                            original.angle - angle_offset_);
    }
  }


  void rotateMatrix(const Angle& rotate_offset)
  {
    matrix<double> rotate_matrix(4, 4);
    double radian = -rotate_offset.to_rad();

    rotate_matrix(0, 0) = cos(radian);
    rotate_matrix(0, 1) = sin(radian);
    rotate_matrix(0, 2) = 0.0;
    rotate_matrix(0, 3) = 0.0;
    rotate_matrix(1, 0) = -sin(radian);
    rotate_matrix(1, 1) = cos(radian);
    rotate_matrix(1, 2) = 0.0;
    rotate_matrix(1, 3) = 0.0;
    rotate_matrix(2, 0) = 0.0;
    rotate_matrix(2, 1) = 0.0;
    rotate_matrix(2, 2) = 1.0;
    rotate_matrix(2, 3) = 0.0;
    rotate_matrix(3, 0) = 0.0;
    rotate_matrix(3, 1) = 0.0;
    rotate_matrix(3, 2) = 0.0;
    rotate_matrix(3, 3) = 1.0;

    offset_ = prod(offset_, rotate_matrix);
    offset_invert_ = prod(rotate_matrix, offset_);
  }


  void moveMatrix(const Position<long>& move_offset)
  {
    matrix<double> move_matrix(4, 4);
    move_matrix(0, 0) = 1.0;
    move_matrix(0, 1) = 0.0;
    move_matrix(0, 2) = 0.0;
    move_matrix(0, 3) = 0.0;
    move_matrix(1, 0) = 0.0;
    move_matrix(1, 1) = 1.0;
    move_matrix(1, 2) = 0.0;
    move_matrix(1, 3) = 0.0;
    move_matrix(2, 0) = 0.0;
    move_matrix(2, 1) = 0.0;
    move_matrix(2, 2) = 1.0;
    move_matrix(2, 3) = 0.0;
    move_matrix(3, 0) = move_offset.x;
    move_matrix(3, 1) = move_offset.y;
    move_matrix(3, 2) = 0.0;
    move_matrix(3, 3) = 1.0;

    offset_ = prod(offset_, move_matrix);
    offset_invert_ = prod(move_matrix, offset_);
  }
};


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


BeegoDrive::~BeegoDrive(void)
{
}


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


long BeegoDrive::timestamp(void) const
{
#if defined(USING_RUN_CTRL)
  return pimpl->run_.getModuleRawTicks();
#else
  return 0;
#endif
}


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);
  const Coordinate* base_coordinate = (coordinate) ? coordinate : root();
  target_position = pimpl->
    adjustPosition(this->pointPosition(base_coordinate, target_position));

  long dir16 = pimpl->convertDir16(target_position.angle);

#if defined(USING_RUN_CTRL)
  VXV::Position pos(target_position.x, -target_position.y,
                    VXV::rad(-2.0 * M_PI * dir16 / 0x10000));
  pimpl->run_.followLine(pos);

#else
  if (! pimpl->command_packet_.followLine(target_position.x,
                                          target_position.y, dir16)) {
    // !!! 例外を投げるべき
  }
#endif
}


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);
  const Coordinate* base_coordinate = (coordinate) ? coordinate : root();
  target_position = this->pointPosition(base_coordinate, target_position);

  long dir16 = pimpl->convertDir16(target_position.angle);

#if defined(USING_RUN_CTRL)
  VXV::Position position(target_position.x,
                         -target_position.y, VXV::deg(dir16));
  pimpl->run_.stopToLine(position);

#else
  if (! pimpl->command_packet_.stopLine(target_position.x,
                                        target_position.y, dir16)) {
    // !!! 例外を投げるべき
  }
#endif
}


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)
{
  // !!! coordinate を使っていないのを修正する

#if defined(USING_RUN_CTRL)
  (void)coordinate;
  (void)center;
  (void)radius;
  // !!!

#else
  Position<long> target_point(center.x, center.y, deg(0));
  const Coordinate* base_coordinate = (coordinate) ? coordinate : root();
  target_point = this->pointPosition(base_coordinate, target_point);

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


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)
{
#if defined(USING_RUN_CTRL)
  pimpl->run_.rotateAngle(VXV::rad(angle_per_sec.to_rad()));

#else
  long dir16 = pimpl->convertDir16(angle_per_sec);
  if (! pimpl->command_packet_.spin(dir16)) {
    // !!! 例外を投げるべき
  }
#endif
}


void BeegoDrive::rotate(const Angle& angle, const Coordinate* coordinate)
{
  Position<long> target_position(0, 0, angle);
  const Coordinate* base_coordinate = (coordinate) ? coordinate : root();
  target_position = this->pointPosition(base_coordinate, target_position);
  long dir16 = pimpl->convertDir16(target_position.angle);

#if defined(USING_RUN_CTRL)
  pimpl->run_.rotateToDirection(VXV::rad(-2.0 * M_PI * dir16 / 0x10000));

#else
  if (! pimpl->command_packet_.rotate(dir16)) {
    // !!! 例外を投げるべき
  }
#endif
}


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


void BeegoDrive::freeServo(void)
{
#if defined(USING_RUN_CTRL)
  pimpl->run_.servoCtrl(false);

#else
  // !!!
#endif
}


qrk::Position<long> BeegoDrive::position(const Coordinate* coordinate) const
{
#if defined(USING_RUN_CTRL)
  VXV::Position run_position = pimpl->run_.getRunPosition();
  Position<long> target_position(run_position.x, -run_position.y,
                                 rad(-run_position.zt.to_rad()));

#else
  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)) のままデータを返す
    //fprintf(stderr, "fail! ");
    return target_position;
  }
  target_position.x = mm[0];
  target_position.y = mm[1];
  target_position.angle = rad(2.0 * M_PI * dir16 / 65536.0);

#endif
  const Coordinate* base_coordinate = (coordinate) ? coordinate : root();
  return pimpl->adjustPosition(base_coordinate->
                               pointPosition(this, target_position), true);
}


long BeegoDrive::maxStraightVelocity(void) const
{
  long max_velocity = 0;
#if defined(USING_RUN_CTRL)
  // !!!

#else
  if (! pimpl->command_packet_.maxStraightVelocity(&max_velocity)) {
    // !!! 例外を投げるべき
  }
#endif

  return max_velocity;
}


long BeegoDrive::straightVelocity(void) const
{
  long velocity = 0;
#if defined(USING_RUN_CTRL)
  // !!!

#else
  if (! pimpl->command_packet_.straightVelocity(&velocity)) {
    // !!! 例外を投げるべき
  }
#endif

  return velocity;
}


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

  return velocity;
#endif

  return deg(0);
}


#if 0
void BeegoDrive::setRobotPosition(const Position<long>& position,
                                  const Coordinate* coordinate)
{
  // 現在のロボット位置と指定されたロボット位置の差分を修正する
  Position<long> current = this->position(coordinate);
  Position<long> diff = position - current;
  //fprintf(stderr, "[%d, %d, %d], ", current.x, diff.x, position.x);

  Position<long> rotate_offset = qrk::rotate<long>(current, diff.angle);

  Position<long> offset = parent()->offset(this);
  offset.x += current.x - rotate_offset.x;
  offset.y += current.y - rotate_offset.y;
  offset.x += diff.x;
  offset.y += diff.y;
  offset.angle += diff.angle;
  //fprintf(stderr, "<%ld, %ld, %d>, ", offset.x, offset.y, offset.angle.to_deg());

  //fprintf(stderr, "(set: %.2f), ", 180.0 * position.rad() / M_PI);
  setOriginTo(parent(), offset);
}
#endif


void BeegoDrive::changeRobotPosition(const Position<long>& to,
                                     const Position<long>& from)
{
  // 回転
  Angle angle_diff = to.angle - from.angle;
  if (angle_diff.to_rad() != 0.0) {
    pimpl->moveMatrix(Position<long>(0, 0, deg(0)) - from);
    pimpl->rotateMatrix(angle_diff);
    pimpl->moveMatrix(from);
    pimpl->angle_offset_ -= angle_diff;
  }

  // 並行移動
  pimpl->moveMatrix(to - from);
}


#if 0
void BeegoDrive::setRobotPositionOffset(const Position<long>& offset)
{
  // 現在のロボット位置と指定されたロボット位置の差分を修正する
  pimpl->offset_ -= offset;
}
#endif


void BeegoDrive::setStraightVelocity(long mm_per_sec)
{
#if defined(USING_RUN_CTRL)
  (void)mm_per_sec;
  // !!!

#else
  if (! pimpl->command_packet_.setStraightVelocity(mm_per_sec)) {
    // !!! 例外を投げるべき
  }
#endif
}


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 defined(USING_RUN_CTRL)
  stable = pimpl->run_.isStable();

#else
  if (! pimpl->command_packet_.isStable(stable)) {
    // !!! 例外を投げるべき
  }
#endif

  return stable;
}


void BeegoDrive::setWheelVelocity(int id, long mm_per_sec)
{
#if defined(USING_RUN_CTRL)
  (void)id;
  (void)mm_per_sec;
  // !!!

#else
  if (! pimpl->command_packet_.setWheelVelocity(id, mm_per_sec)) {
    // !!! 例外を投げるべき
  }
#endif
}


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
