/*!
  \example check_performance.c

  \brief Communication time checker

  \author Satofumi KAMIMURA

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

#include "urg_ctrl.h"
#include <stdio.h>
#include <stdlib.h>


static void urg_exit(urg_t *urg, const char *message)
{
  printf("%s: %s\n", message, urg_error(urg));
  urg_disconnect(urg);

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


int main(int argc, char *argv[])
{
  enum { DataSize = 1081 };
  //const char device[] = "COM4"; /* Windows */
  const char device[] = "/dev/ttyACM0"; /* Linux */

  long data[DataSize];
  int first_index;
  int last_index;
  int front_index;

  int skip_lines = 2;
  int skip_frames = 0;
  int use_partial = 1;

  (void)argc;
  (void)argv;

  urg_t urg;
  if (urg_connect(&urg, device, 115200) < 0) {
    urg_exit(&urg, "urg_connect()");
  }

  // range: -90, +90
  first_index = urg_deg2index(&urg, -90);
  last_index = urg_deg2index(&urg, +90);

  urg_setSkipLines(&urg, skip_lines);
  urg_setSkipFrames(&urg, skip_frames);

  front_index = urg_deg2index(&urg, 0);
  if (use_partial) {
    front_index -= first_index;
    front_index /= skip_lines;
  }

  // using MD
  //urg_requestData(&urg, URG_MD, first_index, last_index);

  // receive data, and check
  while (1) {
    int n;
    int i;

    urg_requestData(&urg, URG_GD, first_index, last_index);

    if (use_partial) {
      n = urg_receivePartialData(&urg, data, DataSize, first_index, last_index);
    } else {
      n = urg_receiveData(&urg, data, DataSize);
    }

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

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

  return 0;
}
