/*!
  \example expand_2d.c

  \brief URG データの２次元展開サンプル

  URG から取得した距離データを、前方が X 軸の正の方向であるとして展開する

  \author Satofumi KAMIMURA

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

#include <math.h>
#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);
}


//! main
int main(int argc, char *argv[])
{
  const char device[] = "COM3"; /* Windows での例 */
  //const char device[] = "/dev/ttyACM0"; /* Linux での例 */

  long *data = NULL;
  int data_max;
  int min_length = 0;
  int max_length = 0;
  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);
  }

  /* GD でのデータ要求 */
  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()");
  }

  /* ２次元データに展開して出力 */
  /* URG 前方を X 軸の正の方向とみなす */
  min_length = urg_minDistance(&urg);
  max_length = urg_maxDistance(&urg);
  for (i = 0; i < n; ++i) {
    int x, y;
    long length = data[i];

    /* 有効範囲外のデータは無視する */
    if ((length <= min_length) || (length >= max_length)) {
      continue;
    }

    x = (int)(length * cos(urg_index2rad(&urg, i)));
    y = (int)(length * sin(urg_index2rad(&urg, i)));

    printf("%d\t%d\t# %d, %ld\n", x, y, i, length);
  }

  urg_disconnect(&urg);
  free(data);

#ifdef MSC
  getchar();
#endif

  return 0;
}
