#ifndef QRK_BEEGO_DRIVE_H
#define QRK_BEEGO_DRIVE_H

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

  \author Satofumi KAMIMURA

  $Id: BeegoDrive.h 1583 2009-12-20 02:12:07Z satofumi $
*/

#include "Coordinate.h"
#include "Running.h"
#include "Timestamp.h"
#include "WheelDirect.h"
#if 0
#include "motor_t.h"
#include "encoder_t.h"
#include "wheel_t.h"
#include "body_t.h"
#endif


namespace qrk
{
  //! ビーゴの制御
  class BeegoDrive
    : public Coordinate, public Running, public Timestamp, public WheelDirect
  {
  public:
    enum {
      DefaultBaudrate = 115200, // [bps]
    };
    BeegoDrive(void);
    ~BeegoDrive(void);

    const char* what(void) const;

    long timestamp(void) const;

    bool connect(const char* device, long baudrate = DefaultBaudrate);
    void disconnect(void);
    bool isConnected(void) const;
    void setConnection(Connection* connection);
    Connection* connection(void);

    void followLine(const Position<long>& line,
                    const Coordinate* coordinate = NULL);
    void followLine(long x, long y, const Angle& angle,
                    const Coordinate* coordinate = NULL);

    void stopLine(const Position<long>& line,
                  const Coordinate* coordinate = NULL);
    void stopLine(long x, long y, const Angle& angle,
                  const Coordinate* coordinate = NULL);

    void followCircle(const Point<long>& center, long radius,
                      const Coordinate* coordinate = NULL);
    void followCircle(long x, long y, long radius,
                      const Coordinate* coordinate = NULL);

    void stopCircle(const Point<long>& center, long radius, const Angle& angle,
                    const Coordinate* coordinate = NULL);

    void spin(const Angle& angle);
    void rotate(const Angle& angle, const Coordinate* coordinate = NULL);

    void stop(void);
    void freeServo(void);

    Position<long> position(const Coordinate* coordinate = NULL) const;
    long maxStraightVelocity(void) const;
    long straightVelocity(void) const;
    Angle rotateVelocity(void) const;

#if 0
    // !!! このメソッドは不要
    void setRobotPosition(const Position<long>& position,
                          const Coordinate* coordinate = NULL);
#endif

    // !!! 比較を同じ座標系で行う、ということにすれば、座標系の指定はいらない
    void changeRobotPosition(const Position<long>& to,
                             const Position<long>& from);

    //void setRobotPositionOffset(const Position<long>& offset);

    void setStraightVelocity(long mm_per_sec);
    void setRotateVelocity(const Angle& angle_per_sec);
    void setStraightAcceleration(long mm_per_sec2);
    void setRotateAcceleration(const Angle& angle_per_sec2);

    bool isStable(void) const;

    void setWheelVelocity(int id, long mm_per_sec);


    /*!
      \brief 現在処理している移動コマンドを返す

      \return 移動コマンドの文字列

      Example
      \code
run.followLine(Position<long>(0, 0, deg(45)));

cout << run.currentCommand() << endl;
// "followLine()" \endcode
    */
    std::string currentCommand(void) const;


    /*!
      \brief 現在処理している移動コマンドをスタックに積む
    */
    void saveCommand(void);


    /*!
      \brief スタック上の移動コマンドを再評価する
    */
    void restoreCommand(void);


    /*!
      \brief パラメータの再計算を走行コントローラに指示する
    */
    void commitParameters(void);


#if 0
    /*!
      \brief モータ・パラメータの登録

      \param[in] id モータ ID
      \param[in] motor モータ情報
    */
    void setMotorParameter(int id, const motor_t& motor);


    /*!
      \brief エンコーダ・パラメータの登録

      \param[in] id エンコーダ ID
      \param[in] encoder エンコーダ情報
    */
    void setEncoderParameter(int id, const encoder_t& encoder);


    /*!
      \brief 車輪パラメータの登録

      \param[in] id 車輪 ID
      \param[in] wheel 車輪の情報
    */
    void setWheelParameter(int id, const wheel_t& wheel);


    /*!
      \brief 筐体パラメータの登録

      \param[in] body 筐体の情報
    */
    void setBodyParameter(const body_t& body);
#endif

  private:
    BeegoDrive(const BeegoDrive& rhs);
    BeegoDrive& operator = (const BeegoDrive& rhs);

    struct pImpl;
    std::auto_ptr<pImpl> pimpl;
  };
}

#endif /* !QRK_BEEGO_DRIVE_H */
