/*!
  \example mdCaptureSample.cpp

  \brief MD コマンドによるデータ取得サンプル

  \author Satofumi KAMIMURA

  $Id: mdCaptureSample.cpp 1437 2009-10-21 16:08:07Z satofumi $
*/

#include "UrgCtrl.h"
#include "delay.h"
#include "ticks.h"
#include <cstdlib>

using namespace qrk;
using namespace std;


//! main
int main(int argc, char *argv[])
{
  const char device[] = "COM3";
  //const char device[] = "/dev/ttyACM0";

  UrgCtrl urg;
  if (! urg.connect(device)) {
    printf("UrgCtrl::connect: %s\n", urg.what());
    exit(1);
  }

#if 1
  // 取得モードを MD に設定
  urg.setCaptureMode(AutoCapture);

#else
  // 強度データ付きのデータ取得モード
  urg.setCaptureMode(IntensityCapture);
  urg.setCaptureSkipLines(2);
#endif
  int scan_msec = urg.scanMsec();

#if 0
  // 取得範囲を 中央から左右方向に 90度(計180度)に設定
  const double rad90 = 90.0 * M_PI / 180.0;
  urg.setCaptureRange(urg.rad2index(-rad90), urg.rad2index(rad90));
#endif

  int pre_timestamp = ticks();

  // MD コマンドで連続データ取得を行い、指定回数だけ出力する
  enum { CaptureTimes = 10};
  urg.setCaptureTimes(CaptureTimes);
  for (int i = 0; i < CaptureTimes;) {
    long timestamp = 0;
    vector<long> data;

    // データ取得
    int n = urg.capture(data, &timestamp);
    if (n <= 0) {
      delay(scan_msec);
      continue;
    }

    // 表示
    printf("timestamp: %ld, (%d), %ld\n",
           timestamp, ticks(), timestamp - pre_timestamp);
    pre_timestamp = timestamp;
#if 0
    for (int j = 0; j < n; ++j) {
      // urg_minDistance() 以下の距離データは、無効値を表す
      printf("%d:%ld, ", j, data[j]);
    }
    printf("\n");
#endif
    ++i;
  }

#ifdef MSC
  getchar();
#endif

  return 0;
}
