/*!
  \example capture_sample.c

  \brief 前方距離データの取得

  \author Satofumi KAMIMURA

  $Id: capture_sample.c 1194 2009-07-31 13:45:24Z satofumi $
*/

#include "urg_ctrl.h"
#include "detect_os.h"
#include <stdio.h>
#include <stdlib.h>
#include <assert.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);
}


static void printData(urg_t *urg, urg_parameter_t *parameter, long data[])
{
  /* 前方の距離データを、タイムスタンプ付きで表示する */
  /* 一定周期以内にデータが取得できないと、タイムスタンプ表示が -1 になる */
  int timestamp = urg_recentTimestamp(urg);

  /* urg_minDistance() 以下の距離データは、無効値を表す */
  printf("%d: %ld [mm], %d [msec]\n",
         parameter->area_front_, data[parameter->area_front_], timestamp);
}


//! main
int main(int argc, char *argv[])
{
  enum {
    CaptureTimes = 10,          /* データ取得回数 */
  };

  const char device[] = "COM3"; /* Windows での例 */
  //const char device[] = "/dev/ttyACM0"; /* Linux での例 */

  int data_max;
  long* data;
  urg_parameter_t parameter;
  int ret;
  int n;
  int i;

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

  /* 受信バッファの確保 */
  data_max = urg_dataMax(&urg);
  data = (long*)malloc(sizeof(long) * data_max);
  if (data == NULL) {
    perror("data buffer");
    exit(1);
  }
  urg_parameters(&urg, &parameter);

  /* GD でのデータ要求。取得回数は CaptureTimes 回 */
  printf("GD capture\n");
  for (i = 0; i < CaptureTimes; ++i) {
    /* データ要求 */
    ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
    if (ret < 0) {
      urg_exit(&urg, "urg_requestData()");
    }

    /* データ受信 */
    n = urg_receiveData(&urg, data, data_max);
    if (n < 0) {
      urg_exit(&urg, "urg_receiveData()");
    }

    /* 表示 */
    printData(&urg, &parameter, data);
  }
  printf("\n");


  /* MD でのデータ要求。取得回数は CaptureTimes 回 */
  printf("MD capture\n");

  /* 100 回以上の連続データを取得するには、取得回数を無限回に設定すること */
  /* urg_setCaptureTimes(&urg, UrgInfinityTimes); */
  assert(CaptureTimes < 100);
  urg_setCaptureTimes(&urg, CaptureTimes);

  /* データ要求 */
  ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
  if (ret < 0) {
    urg_exit(&urg, "urg_requestData()");
  }

  for (i = 0; i < CaptureTimes; ++i) {
    /* 受信 */
    n = urg_receiveData(&urg, data, data_max);
    if (n < 0) {
      urg_exit(&urg, "urg_receiveData()");
    }

    /* 表示 */
    printData(&urg, &parameter, data);
  }

  if (CaptureTimes > 99) {
    /* 99 回以上のデータ取得の場合、明示的なデータ取得の停止が必要 */
    urg_laserOff(&urg);
  }

  urg_disconnect(&urg);
  free(data);

#ifdef MSC
  getchar();
#endif

  return 0;
}
