/*!
  \example setTimestampSample.cpp

  \brief タイムスタンプ合わせの動作サンプル

  \author Satofumi KAMIMURA

  $Id: setTimestampSample.cpp 1905 2010-09-13 01:36:15Z satofumi $
*/

#include "mUrgDevice.h"
#include "ticks.h"
#include "delay.h"
#include <SDL.h>

using namespace qrk;
using namespace std;


int main(int argc, char *argv[])
{
  mUrgDevice urg(argc, argv);

  if (! urg.connect("/dev/ttyACM0")) {
    printf("UrgDevice::connect: %s\n", urg.what());
    exit(1);
  }
  //urg.setCaptureMode(AutoCapture);

  long pc_timestamp = ticks();
  urg.setTimestamp(pc_timestamp);
  printf("PC first timestamp: %ld\n", pc_timestamp);

  long timestamp = 0;
  vector<long> data;
  //urg.capture(data, &timestamp);
  //printf("timestamp: %ld\n", timestamp);

  // 現在の PC タイムスタンプと URG のタイムスタンプを同期させる
  for (int i = 0; i < 5;) {
      int n;
      do {
          pc_timestamp = ticks();
          n = urg.capture(data, &timestamp);
          delay(1);
      } while (n <= 0);
      if (n > 0) {
          printf("PC : %ld,\t", pc_timestamp);
          printf("URG: %ld\n", timestamp);
          ++i;
      }
  }

  return 0;
}
