/*!
  \example multiMdScanSample.cpp

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

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

  \author Satofumi KAMIMURA

  $Id: multiMdScanSample.cpp 1739 2010-03-06 07:43:32Z satofumi $
*/

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

using namespace qrk;
using namespace std;


namespace
{
  void urg_exit(int i, mUrgDevice* urg, const char *message)
  {
    printf("%d: %s: %s\n", i, message, urg->what());
    urg->disconnect();

    exit(1);
  }


  bool remainTimes(int remain_times[], size_t n)
  {
    for (size_t i = 0; i < n; ++i) {
      if (remain_times[i] > 0) {
        return true;
      }
    }
    return false;
  }
}


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

  enum {
    Times = 10,
    Devices = 2,
  };

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

  // 接続
  mUrgDevice urg_0(argc, argv);
  mUrgDevice urg_1(argc, argv);
  mUrgDevice* urg[Devices];
  urg[0] = &urg_0;
  urg[1] = &urg_1;

  for (int i = 0; i < Devices; ++i) {
    if (! urg[i]->connect(connect_devices[i])) {
      urg_exit(i, urg[i], "UrgDevice::connect()");
    }

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

  long pre_timestamp[Devices];
  string error_messages[Devices];
  int remain_times[Devices];
  for (int i = 0; i < 2; ++i) {
    pre_timestamp[i] = 0;
    remain_times[i] = Times;
  }

  while (remainTimes(remain_times, Devices)) {

    delay(1);

    for (int i = 0; i < Devices; ++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) {
        printf("%d:%ld, ", j, data[i][j]);
      }
      printf("\n\n");
#endif
    }
  }

  return 0;
}
