/*!
  \example multiMdScanSample.cpp

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

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

  \author Satofumi KAMIMURA

  $Id: multiMdScanSample.cpp 1043 2009-06-19 14:51:09Z satofumi $
*/

#include "UrgCtrl.h"
#include "delay.h"
#include <SDL.h>

using namespace qrk;
using namespace std;


static void urg_exit(int i, UrgCtrl& urg, const char *message) {

  printf("%d: %s: %s\n", i, message, urg.what());
  urg.disconnect();

  exit(1);
}


//! main
int main(int argc, char *argv[]) {

  enum {
    Times = 10,
  };

  //const char *connect_devices[] = { "COM3", "COM4" };
  const char *connect_devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" };

  // 接続
  UrgCtrl urg[2];
  for (int i = 0; i < 2; ++i) {
    if (! urg[i].connect(connect_devices[i])) {
      urg_exit(i, urg[i], "UrgCtrl::connect()");
    }

    // データを間引くと、連続転送が容易になる
    // 間引き指定が 2 だと、データ転送量は半分になる
    //urg.setCaptureSkipLines(2);
    urg[i].setCaptureTimes(Times);
    urg[i].setCaptureMode(AutoCapture);
  }

  long pre_timestamp[2] = { 0, 0 };
  string error_messages[2];
  int remain_times[2] = { Times, Times };
  while ((remain_times[0] > 0) || (remain_times[1] > 0)) {

    delay(1);

    for (int i = 0; i < 2; ++i) {
      if (remain_times[i] <= 0) {
        continue;
      }

      /* データ取得 */
      vector<long> data;
      long timestamp = 0;
      int n = urg[i].capture(data, &timestamp);
      if (n < 0) {
        // 次回に受信できるかもしれないので、処理を続ける
        string error = urg[i].what();
        if (error_messages[i].compare(error)) {
          // 前回と違うメッセージのときのみ、表示する
          printf("    %d: %s: %s\n", i, "capture()", error.c_str());
          error_messages[i] = error;
        }
        continue;

      } else if (n == 0) {
        continue;
      }
      --remain_times[i];

      printf("    %d: ", i);
      /* データ取得が完了していたら、終了 */
      remain_times[i] = min(static_cast<int>(urg[i].remainCaptureTimes()),
                            remain_times[i]);
      printf("(%03d/%03d): ", remain_times[i], Times);

      /* 表示 */
      printf("timestamp: %ld (%ld)", timestamp, (timestamp - pre_timestamp[i]));
      printf("\n");
      pre_timestamp[i] = timestamp;

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

  return 0;
}
