/*!
  \example gd_scan.c

  \brief Sample of G scan

  Get distance data continuously using GD command

  \author Satofumi KAMIMURA

  $Id: g_scan.c 1989 2012-05-18 00:32:36Z 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 { SCAN_TIMES = 2 };
#ifdef WINDOWS_OS
  const char device[] = "COM3"; /* For Windows */
#else
  const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif

  int data_max;
  long *data;
  int ret;
  int n;
  int i;
  int k;

  /* Connection */
  urg_t urg;
  urg_initialize(&urg);
  ret = urg_connect(&urg, device, 115200);
  if (ret < 0) {
    urg_exit(&urg, "urg_connect()");
  }

  /* Reserve for reception data */
  data_max = urg_dataMax(&urg);
  data = (long*)malloc(sizeof(long) * data_max);
  if (data == NULL) {
    perror("malloc");
    exit(1);
  }

  for (i = 0; i < SCAN_TIMES; ++i) {
      /* Request for GD data */
      ret = urg_requestData(&urg, URG_G, URG_FIRST, URG_LAST);
      if (ret < 0) {
          urg_exit(&urg, "urg_requestData()");
      }

      /* Reception */
      n = urg_receiveData(&urg, data, data_max);
      printf("# n = %d\n", n);
      if (n < 0) {
          urg_exit(&urg, "urg_receiveData()");
      }
  }

  /* Display */
  for (i = 0; i < SCAN_TIMES; ++i) {
      for (k = 0; k < n; ++k) {
          /* Neglect the distance less than  urg_minDistance()  */
          printf("%ld, ", data[k]);
      }
      printf("\n");
  }

  urg_disconnect(&urg);
  free(data);

#ifdef MSC
  getchar();
#endif

  return 0;
}
