#ifndef VXV_RUN_CTRL_H
#define VXV_RUN_CTRL_H

/*!
  \file
  \brief 走行制御クラス

  \author Satofumi KAMIMURA

  $Id: runCtrl.h 439 2009-01-03 05:01:55Z satofumi $
*/

#include "runInterface.h"
#include "ticksCtrlInterface.h"
#include "structTables.h"
#include "connectionDevice.h"
#include "nodeAccess.h"
#include "timeUtils.h"
#include <list>

namespace qrk
{
  class Connection;
}


/*!
  \brief 走行制御クラス
*/
namespace VXV
{
  class RunCtrl : public ObjCoordinateCtrl, public RunInterface,
                  public TicksCtrlInterface {
    friend class PacketHandleHostTest;
    friend class tRunCtrl_Simulator;
    friend class MonitorTask;

  protected:
    enum {
      SimulatorPort = 49754,
    };

    /*!
      \brief 初期設定
    */
    enum Default {
      Baudrate = 115200,        /*!< 接続ボーレート */
      StraightVel = 300,        /*!< 並進速度 */
      StraightAcc = 600,        /*!< 並進加速度 */
      RotateVel = 0x10000 >> 1,	/*!< 回転角速度 */
      RotateAcc = 0x10000 >> 1,	/*!< 回転角加速度 */
      FollowRadius = 500,       /*!< 経路追従時の曲率 */
    };

  private:
    qrk::Connection* con;
    nodeInfo_t* node;
    long unique_id;
    bool crd_auto;
    nodeInfo_t nodeResource;

  protected:
    VXV::Position local_offset;	/*!< ロボット位置の修正用 */

  private:
    std::string error_message;

    typedef struct {
      int straight_ref_vel;
      int straight_ref_acc;
      long rotate_ref_vel;
      long rotate_ref_acc;
      int follow_r;
    } runParams_t;

    enum { SEND_COMMAND_SIZE = 256 };
    typedef struct {
      VXV::Position position;
      const CoordinateCtrl* crd;
      int send_command_size;
      char send_command[SEND_COMMAND_SIZE];
    } runCommand_t;

    typedef struct {
      runParams_t params;
      runCommand_t command;
    } runState_t;

    std::list<runState_t> state_stack;

    unsigned long pre_module_msec;
    unsigned long total_msec;
    ticksInfo_t ticksInfoObj;
    ticksInfo_t& getTicksInfo(void) {
      return ticksInfoObj;
    }
    void beginTimeAdjust(void);
    void endTimeAdjust(void);

    void initRunParams(void);
    void printHelp(void);
    void printVersion(void);
    bool parseArgs(int* ret_value, int argc, char *argv[]);
    int checkVersion(void);
    void crdNortify(int state, const CoordinateCtrl* crd);
    const VXV::Position getRunCmdPos(const CoordinateCtrl* crd,
                                     const VXV::Position& position);
    void sendUpdatedPosition(const CoordinateCtrl* crd,
                             const VXV::Position& position);
    unsigned long updateTicksDiff(unsigned long ticks);
    RunCtrl(const RunCtrl& rhs);

#include "commandCtrl.h"

  protected:
    runCtrl_t* tbl;		/*!< 走行制御パラメータ構造体 */
    runCtrl_t tblResource;

    /*!
      \brief Direction 値を div16 角度に変換

      走行制御コントローラに角度を渡す際の変換に使用

      \param t [i] 角度 [VXV::Direction]

      \retval div16 値
    */
    int to_div16(const VXV::Direction& t);

  public:
    unsigned long getModuleRawTicks(void);

    /*!
      \brief 接続処理

      接続処理を行う

      \param conObj [i] 接続オブジェクト
      \param device [i] 接続デバイス
      \param baudrate [i] 接続ボーレート

      \retval 0 正常終了
      \retval 戻り値 < 0 エラー
    */
    virtual int raw_connect(qrk::Connection* conObj,
                            const char* device, long baudrate);

    /*!
      \brief ホスト側のタイムスタンプ取得
    */
    virtual unsigned long getHostTicks(void);

    /*!
      \brief 時刻合わせ処理の初期化
    */
    virtual void initTicksInfo(void);

  public:
    enum {
      LineOver = +1, LineNotOver = -1,
      DefaultTimeout = -1,
    };
    CoordinateCtrl& FS;		/*!< 筐体位置中心の座標系 */

    /*!
      \defgroup system_grp システムコマンド
      \{
    */
    RunCtrl(void);
    virtual ~RunCtrl(void);

    const char* what(void);

    void setConnection(qrk::Connection* connection);

    virtual int connect(int argc, char *argv[]);

    virtual int connect(const char* device, long baudrate = Baudrate);

    /*!
      \brief 設定ファイルの内容を評価して接続を行う

      設定ファイルである defaultargs という名前のファイル内容を評価して接続を行う。defaultargs ファイルは ./, ~/.vxv/ という順番に走査される。defaultargs ファイルには、渡したい引数を１行毎に記述しておくこと。defaultargs の '#' から行末までの内容は無視される\n
      このメソッドは、connect(argc, argv) で接続が確立できなかったときにも評価される

      ~/.vxv/defaultargs の例
      \code
      # vxv2
      --run_port=/dev/ttyUSB0
      --urg_port=/dev/ttyACM0
      --urg_handstand
      \endcode

      \code
      include <runCtrl.h>

      int main(int argc, char *argv[]) {
        try {
          RunCtrl run;
	  if (run.connect(argc, argv) < 0) {
            printf("RunCtrl::connect: %s\n", run.what());
	    exit(1);
	  }
	  ...

        } chatch (std::exception& e) {
          printf("exception: %s\n", e.what());
        }
        return 0;
      }
      \endcode
    */
    virtual int connect(void);

    /*!
      \brief ソケットに対して接続

      TCP/IP 接続を行う。通信内容は、走行制御コントローラに送るデータを同じとなる。シミュレータに対して接続したい場合や、ネットワーク越しに走行制御コントローラを制御したい場合に用いる

      \param host [i] 接続ホスト
      \param port [i] 接続ポート

      \retval 0 正常終了
      \retval 戻り値 < 0 エラー
    */
    int connectSocket(const char* host, long port = SimulatorPort);

    virtual void disconnect(void);

    /*!
      \brief 接続されているかどうかを返す

      走行制御オブジェクトがポートに対して接続されているかを返す

      \retval true 接続されている
      \retval false 接続されていない

      使用例
      \code
      RunCtrl run;
      run.connect(argc, argv);
      if (!run.isConnect()) {
        printf("RunCtrl::connect: %s\n", run.what());
        exit(1);
      }
      ...
      \endcode
    */
    virtual bool isConnected(void);

    void push_runState(void);
    void pop_runState(void);


    /*!
      \brief 通信エラー時の再送回数設定

      走行制御コントローラとの通信エラーが起こった場合、１トランザクションにおいて何回失敗したらアボートするかを設定する

      \param times [i] 再送回数
    */
    void set_sendRetryTimes(int times);

    /*!
      \brief 受信時のタイムアウトを指定

      走行制御コントローラからの応答がこなかった場合、何 msec だけ応答が来なければ通信エラーと見なすかを設定する

      \param timeout [i] タイムアウト時間 [msec]
    */
    void set_recvTimeout(int timeout = DefaultTimeout);

    /*!
      \brief msec Watch Dog Timer 時間の設定

      走行制御コントローラは、設定時間毎に何らかの通信が届かなければ、停止コマンドを発行して以後動作しなくなる。停止するまでのカウンタは、任意の通信コマンドが走行制御コントローラに届く度に設定値にリセットされる

      \param msec [i] タイムアウトまでの時間
    */
    void set_watchDogTimer(unsigned long msec);

    /*! \} */


    /*!
      \defgroup position_grp ロボット位置操作コマンド
      \{
    */

    virtual VXV::Position3D getLocalPosition(void);
    VXV::Position getRunPosition(const CoordinateCtrl* crd = VXV::GL);
    void adjustRunPosition(const VXV::Position& position,
                           const CoordinateCtrl* crd = VXV::GL);
    void coordinateUpdateDetect(bool on);

    /*! \} */

    /*!
      \defgroup move_grp 移動コマンド
      \{
    */

    virtual void stop(void);
    virtual void followLine(const VXV::Position& position,
                            const CoordinateCtrl* crd = VXV::GL);
    virtual void followCircle(const VXV::Grid& center, int radius,
                              const CoordinateCtrl* crd = VXV::GL);
    void followCircleOnTangent(const VXV::Position& position, int radius,
                               const CoordinateCtrl* crd = VXV::GL);
    virtual void stopToLine(const VXV::Position& position,
                            const CoordinateCtrl* crd = VXV::GL);
    virtual void rotateToDirection(const VXV::Direction& direction,
                                   const CoordinateCtrl* crd = VXV::GL);

    virtual void rotateAngle(const VXV::Direction& direction);
    virtual void spin(const VXV::Direction& velocity);
    virtual void lastMoveCommand(const CoordinateCtrl* crd = VXV::GL);

    /*! \} */


    /*!
      \defgroup judge_grp 判定コマンド
      \{
    */

    virtual bool isStable(void);
    int getLengthToGrid(const VXV::Grid& grid,
                        const CoordinateCtrl* crd = VXV::GL);
    int getLengthToBody(const VXV::Grid& grid,
                        const CoordinateCtrl* crd = VXV::GL);
    int getLengthToLine(const VXV::Position& line,
                        const CoordinateCtrl* crd = VXV::GL);
    VXV::Direction getAngleToDirection(const VXV::Direction& t,
                                       const CoordinateCtrl* crd = VXV::GL);
    int getStraightVelDiff(int mm_vel = 0);
    VXV::Direction getRotateVelDiff(const VXV::Direction& t = VXV::Direction());

    /*! \} */


    /*!
      \defgroup parameter_grp パラメータ操作コマンド
      \{
    */

    void setStraightRefVel(int mm_sec);
    void setStraightRefAcc(int mm_acc);
    void setRotateRefVel(const VXV::Direction& rotate_vel);
    void setRotateRefAcc(const VXV::Direction& rotate_acc);
    void setCurveRadius(int mm);
    int getStraightVel(void);
    VXV::Direction getRotateVel(void);
    int getCurveRadius(void);

    /*! \} */


    /*!
      \defgroup direct_grp 直接制御コマンド
      \{
    */

    virtual void servoCtrl(bool on);

    /*!
      \brief PWM の直接指定

      指定モータに対して、出力する PWM のデューティ比を直接指定する

      \param id [i] モータID
      \param duty [i] PWM 比 [0, 255]
    */
    void setMotorPwm(int id, unsigned char duty);

    /*!
      \overload

      \param id [i] モータID
      \param duty [i] PWM 比を格納した配列
      \param num [i] モータID から何個分のモータの PWM 比を指定するか
    */
    void setMotorPwm(int id, unsigned char duty[], int num);

    /*!
      \brief モータの回転モードを直接指定

      モータの回転モードを指定する。指定できる回転モードは、FREE, CW, CWW のいずれか

      \param id [i] モータID
      \param mode [i] モータの制御モード

      \todo FREE の指定が動作していないと思われる、検証する
    */
    void setMotorMode(int id, unsigned char mode);

    /*!
      \overload

      \param id [i] モータID
      \param mode [i] 制御モードを格納した配列
      \param num [i] モータID から何個分のモータの制御モードを指定するか
    */
    void setMotorMode(int id, unsigned char mode[], int num);

    /*!
      \brief エンコーダ速度の取得

      1 [msec]あたりのエンコーダ値の変化量(速度)を取得する

      \param id [i] エンコーダID
      \param cnt [o] エンコーダ値の変位 [1]
    */
    void getEncoderVel(int id, int *cnt);

    /*!
      \overload

      \param id [i] エンコーダID
      \param cnt [o] エンコーダ速度を格納するための配列
      \param num [i] エンコーダID から何個分のエンコーダ速度を取得するか
    */
    void getEncoderVel(int id, int cnt[], int num);

    /*!
      \brief エンコーダカウンタ値の取得

      現時点でのエンコーダカウンタ値を取得する

      \attention 取得間隔によっては、エンコーダのオーバーフローを検出できない

      \param id [i] エンコーダID
      \param cnt [o] エンコーダのカウンタ値 [1]
    */
    void getEncoderValue(int id, unsigned short *cnt);

    /*!
      \overload

      \param id [i] エンコーダID
      \param cnt [o] エンコーダ値を格納するための配列
      \param num [i] エンコーダID から何個分のエンコーダ値を取得するか
    */
    void getEncoderValue(int id, unsigned short cnt[], int num);

    /*!
      \brief 車輪の移動速度を直接指定

      車輪の移動速度を取得する

      \param id [i] 車輪ID
      \param mm_vel [i] 車輪の移動速度 [mm/sec]
    */
    void setWheelVel(int id, int mm_vel);

    /*!
      \overload

      \param id [i] 車輪ID
      \param mm_vel [i] 車輪の移動速度を格納した配列
      \param num [i] 車輪ID から何個分の移動速度を指定するか
    */
    void setWheelVel(int id, int mm_vel[], int num);

    /*! \} */


    /*!
      \defgroup noreset_grp 筐体依存の情報取得コマンド
      \{
    */

    VXV::Position getNoResetRunPosition(void);
    int getBodyRotateCount(void);
    void getEncoderRotateCount(int id, int count[], int num = 1);

    /*! \} */
  };


  /*!
    \brief 走行制御モジュールの例外
  */
  class RunCtrl_Exception : public std::exception {
    std::string error_message;

  public:

    /*!
      \addtogroup system_grp
      \{
    */

    /*!
      \brief メッセージを指定して例外を投げる
    */
    RunCtrl_Exception(const char* message) throw () : error_message(message){}
    virtual ~RunCtrl_Exception(void) throw () {}

    /*!
      \brief 例外原因の文字列を返す

      走行制御モジュール例外を返した理由を表示する

      使用例
      \code
      include <runCtrl.h>

      int main(int argc, char *argv[]) {
        try {
          RunCtrl run;
          ...

        } chatch (std::exception& e) {
          printf("exception: %s\n", e.what());
        }
        return 0;
      }
      \endcode
    */
    virtual const char* what(void) const throw() {
      return error_message.c_str();
    }
    /*! \} */
  };
}

#endif /* !VXV_RUN_CTRL_H */
