/*!
  \example check_performance.c

  \brief Communication time checker

  \author Satofumi KAMIMURA

  $Id: check_performance.cpp 1333 2009-09-23 01:43:49Z satofumi $
*/

#include "UrgCtrl.h"
#include <cstdio>
#include <cstdlib>

using namespace qrk;
using namespace std;


namespace
{
  void urg_exit(UrgCtrl& urg, const char *message)
  {
    printf("%s: %s\n", message, urg.what());

#ifdef MSC
    getchar();
#endif
    exit(1);
  }
}


int main(int argc, char *argv[])
{
  static_cast<void>(argc);
  static_cast<void>(argv);

  enum { DataSize = 1081 };
  //const char device[] = "COM3"; /* Windows */
  const char device[] = "/dev/ttyACM0"; /* Linux */

  int skip_lines = 2;
  int skip_frames = 0;

  UrgCtrl urg;
  if (! urg.connect(device)) {
    urg_exit(urg, "urg_connect()");
  }

  // range: -90, +90
  int first_index = urg.deg2index(-90);
  int last_index = urg.deg2index(+90);
  urg.setCaptureRange(first_index, last_index);

  urg.setCaptureSkipLines(skip_lines);
  urg.setCaptureFrameInterval(skip_frames);

  int front_index = urg.deg2index(0);

  // using MD
  //urg.setCaptureMode(AutoCapture);
  urg.setCaptureMode(ManualCapture);

  // receive data, and check
  vector<long> data;
  while (1) {

    int n = urg.capture(data);

    if (n > 0) {
      printf("n = %d, %ld [mm]\n", n, data[front_index]);
    }

#if 0
    for (int i = 0; i < n; ++i) {
      printf("%d, ", data[i]);
    }
    printf("\n");
#endif
  }

  return 0;
}
