/*!
  \example captureIntensitySample.cpp

  \brief 強度データの取得サンプル

  \author Satofumi KAMIMURA

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

#include "UrgCtrl.h"
#include "delay.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);
  }
  int scan_msec = urg.scanMsec();
  urg.setCaptureMode(IntensityCapture);

  enum {
    CaptureTimes = 10,
  };

  vector<long> data;
  vector<long> intensity_data;

  for (int i = 0; i < CaptureTimes; ++i) {
    long timestamp = 0;

    // 強度付きデータの取得
    int data_n = urg.captureWithIntensity(data, intensity_data, &timestamp);

    if (data_n > 0) {
      int front_index = urg.rad2index(0.0);

      // 表示
      // urg_minDistance() 以下の距離データは、無効値を表す
      printf("%d: %ld [mm] (%ld), %ld [msec]\n",
             i, data[front_index], intensity_data[front_index], timestamp);
    }
    delay(scan_msec);
  }

#ifdef MSC
  getchar();
#endif

  return 0;
}
