/*!
  \example multi_urg.c

  \brief 複数 URG を用いてデータ取得を行うサンプル

  ２台の URG を用いて、それぞれの前方データを表示する

  \author Satofumi KAMIMURA

  $Id: multi_urg.c 1194 2009-07-31 13:45:24Z 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);
}


//! main
int main(int argc, char *argv[])
{
  enum {
    Times = 10,
    Urgs = 2,
  };

  urg_t urg[Urgs];
  long *data[Urgs];
  int data_max[Urgs];
  int timestamp;
  int ret;
  int n;
  int i;
  int k;

  const char *devices[] = { "COM3", "COM4" }; /* Windows 用 */
  //const char *devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" }; /* Linux 用 */

  /* 接続 */
  for (i = 0; i < Urgs; ++i) {
    ret = urg_connect(&urg[i], devices[i], 115200);
    if (ret < 0) {
      urg_exit(&urg[i], "urg_connect()");
    }
    /* 既存の MD コマンドのクリア用 */
    urg_laserOff(&urg[i]);

    /* データを間引くと、連続転送が容易になる */
    /* 間引き指定が 2 だと、データ転送量は半分になる */
    //urg_setSkipLines(&urg[i], 2);

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

  /* MD でのデータ要求 */
  for (i = 0; i < Urgs; ++i) {
    urg_setCaptureTimes(&urg[i], Times);

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

  for (k = 0; k < Times; ++k) {
    for (i = 0; i < Urgs; ++i) {
      /* データ取得が完了していたら、終了 */
      int remain_times = urg_remainCaptureTimes(&urg[i]);
      printf("    %d: ", i);
      printf("(%03d/%03d): ", remain_times, Times);
      if (remain_times <= 0) {
        printf("\n");
        continue;
      }

      /* 受信 */
      n = urg_receiveData(&urg[i], data[i], data_max[i]);
      if (n < 0) {
        // 次回に受信できるかもしれないので、処理を続ける
        printf("%s: %s\n", "urg_receiveData()", urg_error(urg));

      } else {

        /* 表示 */
        timestamp = urg_recentTimestamp(&urg[i]);
        printf("timestamp: %d, ", timestamp);
#if 0
        {
          int j;
          for (j = 0; j < n; ++j) {
            /* urg_minDistance() 以下の距離データは、無効値を表す */
            printf("%d:%ld, ", j, data[i][j]);
          }
          printf("\n");
        }
#endif
        printf("\n");
      }
    }
  }

  /* 切断 */
  for (i = 0; i < Urgs; ++i) {
    urg_disconnect(&urg[i]);
    free(data[i]);
  }

#ifdef MSC
  getchar();
#endif

  return 0;
}
