/// Copyright 2011 Tsuyoshi Suenaga, Robotics Lab. NAIST
/// All rights researved
///
/// The distribution policy is described in the file COPYING.txt furnished
/// with this library.
///
/// Time-stamp: <2011-02-04 09:38:54 tsuyo-s>

#ifndef __MARKERDETECT_H__
#define __MARKERDETECT_H__

#include <GL/gl.h>
#include <GL/glut.h>
#include <AR/arMulti.h>
#include <map>
#include <vector>
#include <string>

/// マーカ検出クラス
class MarkerDetect {
 public:
  /// マーカセット構造体
  /// 1つのマーカセットには複数のARマーカが含まれ、それら全ての相対位置・姿勢は
  /// 固定として扱われる。位置姿勢を推定したい物体1つにつき、1つのマーカセットを
  /// 対応付けて利用することを想定している。
  typedef struct {
    // int id;

    /// 推定された位置姿勢の誤差に相当する値
    double err;

    /// 複数マーカ情報の格納変数
    ARMultiMarkerInfoT *config;
  }MarkerSet;

  /// RGBカラー構造体
  typedef struct {
    unsigned char r;  // red
    unsigned char g;  // green
    unsigned char b;  // blue
  }RGBColor;

  /// コンストラクタ
  MarkerDetect();
  /// デストラクタ
  virtual ~MarkerDetect();

  /// ARToolKitフォーマットでのカメラパラメータファイルの読み込み
  /// @param cparam_name カメラパラメータファイル名
  /// @param num         カメラパラメータに含まれるカメラ数
  /// @return 成功すると0を、失敗すると-1を返す
  int LoadCameraParameter(const std::string &cparam_name,
                          const int &num);
  /// カメラパラメータの設定
  /// @param xsize       画像サイズ幅
  /// @param ysize       画像サイズ高さ
  /// @param mat         射影行列（3x4行列）
  /// @param dist_factor レンズ歪パラメータ(中心x, 中心y, 歪係数, スケール係数)
  /// @return 常に0を返す
  int SetCameraParameter(const int &xsize, const int &ysize,
                         const double mat[][4],
                         const double dist_factor[]);
  /// カメラパラメータの設定
  /// @param xsize       画像サイズ幅
  /// @param ysize       画像サイズ高さ
  /// @param fx          焦点距離x
  /// @param fy          焦点距離y
  /// @param cx          画像中心x
  /// @param cy          画像中心y
  /// @param dist_cx     レンズ歪パラメータ(歪中心x)
  /// @param dist_cy     レンズ歪パラメータ(歪中心y)
  /// @param dist_factor レンズ歪パラメータ(歪係数)
  /// @param dist_scale  レンズ歪パラメータ(スケール係数)
  /// @return 常に0を返す
  int SetCameraParameter(const int &xsize, const int &ysize,
                         const double &fx, const double &fy,
                         const double &cx, const double &cy,
                         const double &dist_cx,
                         const double &dist_cy,
                         const double &dist_factor,
                         const double &dist_scale);

  /// カメラパラメータの表示
  /// @param comment 出力する行列の前に出力する文字列
  void PrintCameraParameter(const std::string comment);

  /// ARマーカの設定ファイルの読み込み。
  /// 複数回呼び出された場合、前回のマーカ情報に追記される。
  /// 既に同じIDのマーカセットが登録されていた場合は登録処理に失敗する。
  /// @param config_filename マーカ設定ファイル名
  /// @return 成功すると0を、失敗すると負の値を返す。エラー番号は以下の通り。
  ///         -1 : ファイル(config_filename)を開くことができない
  ///         -2 : ファイル(config_filename)に記述されたARマーカセットの
  ///              configファイルを開くことができない。
  ///         -3 : 同じIDのARマーカセットが登録された。
  int LoadMarkersConfig(const std::string &config_filename);

  /// 登録されたARマーカ情報のリセット
  void ResetMarkersConfig(void);

  /// ARマーカの設定を表示
  void PrintMarkersConfig(void);

  /// 登録されたマーカID番号の取得
  /// @param id 受け取るマーカID
  void GetRegisteredMarkerIDs(std::vector<int> *id);

  /// 検出されたマーカID番号の取得
  /// @param id 受け取るマーカID
  void GetDetectedMarkerIDs(std::vector<int> *ids);

  /// マーカIDから登録の成否を問い合わせる
  /// @param id 問い合せたいマーカセットID
  /// @return 指定IDのマーカセットが登録されている場合trueを、
  ///         マーカセットが未登録の場合falseを返す
  bool QueryRegistState(const int id);

  /// マーカIDから検出の成否を問い合わせる
  /// @param id 問い合せたいマーカセットID
  /// @return 指定IDのマーカセットが検出されている場合trueを、
  ///         マーカセットが未検出または未登録の場合falseを返す
  bool QueryDetectState(const int id);

  /// ARマーカの検出
  /// @param image     ARマーカ検出を行う画像
  /// @param threshold 検出前処理の二値化の閾値(0-255)
  /// @return 成功すると0を、失敗すると-1を返す
  int Detect(const unsigned char *image, const int &threshold);

  /// 可視のARマーカ上の矩形を描画（この関数はOpenGLを利用しない）
  /// @param image 矩形を書き込む画像
  void DrawVisibleMarkers(unsigned char *image);
  /// PPM画像
  void SavePPMImage(unsigned char *image, char *filename);


  /// OpenGLを利用した描画機能の初期化
  /// ただし事前に、glutInit()によるGLUTの初期化および、
  /// LoadCameraParameter または SetCameraParameter のカメラパラメータ設定を
  /// 呼び出す必要がある。また、GLEWを利用するため事前にGLEWの導入が必要である。
  /// @return 成功すると0を、失敗すると負の値を返す。エラー番号は以下の通り。
  ///         -1 : カメラパラメータが未設定
  ///         -2 : Framebuffer objectが利用できない
  int GlDrawInit(void);
  /// OpenGLを利用した描画機能の終了処理
  void GlDrawFinal(void);

  /// 検出されたARマーカの上に矩形を描画
  /// @param image 描画を行う画像データ
  void GlDraw2D(unsigned char *image);

  /// 推定されたARマーカの上に立方体を描画
  /// 可視なマーカは青色、不可視なマーカは赤色として描画される
  /// @param image 描画を行う画像データ
  void GlDraw3D(unsigned char *image);

#ifdef _DEBUG
  /// ARToolKitのビデオ機能の初期化
  /// @param config ビデオ設定の文字列
  /// @return 成功すると0を、失敗すると負の値を返す。エラー番号は以下の通り。
  ///         -1 : ビデオデバイスを開くことができない
  ///         -2 : キャプチャを開始できない
  int VideoInit(char *config);
  /// ARToolKitのビデオ機能の終了処理
  /// @return 成功すると0を、失敗すると負の値を返す。エラー番号は以下の通り。
  ///         -1 : ビデオデバイスを閉じることができない
  ///         -2 : キャプチャを停止できない
  int VideoFinal(void);

  /// ARToolKitのビデオキャプチャ
  /// @return 成功すると画像へのポインタを、失敗するとNULLを返す
  unsigned char * VideoGetImage(void);
#endif  // _DEBUG

  /// マーカ情報
  std::map<int, MarkerSet> markers;
  /// カメラパラメータ
  ARParam camera_param;


 private:
  /// 直線を描画する（この関数はOpenGLを利用しない）
  void DrawLine(const int x1, const int y1,
                const int x2, const int y2,
                const int width, const int height,
                MarkerDetect::RGBColor color,
                unsigned char *image);
  /// 矩形を描画する（この関数はOpenGLを利用しない）
  void DrawSquare(const double vertex[4][2],
                  const int width, const int height,
                  MarkerDetect::RGBColor color,
                  unsigned char *image);

  /// FrameBufferObject(FBO)の初期化
  /// @return 成功すると0を、失敗すると-1を返す
  int FrameBufferObjectInit(void);
  /// FrameBufferObjectの終了処理
  void FrameBufferObjectFinal(void);

  GLuint texture_name;        // FBO用テクスチャ
  GLuint framebuffer_name;    // FBO用フレームバッファ
  ARMarkerInfo *marker_info;  // 画像に含まれる検出ARマーカ情報
  int           marker_num;   // 画像に含まれる検出ARマーカ個数
  bool gl_draw_init_flag;
};

#endif  /* __MARKERDETECT_H__ */
