/*!
  \example connectById.cpp
  \brief シリアル ID を用いた接続サンプル

  \author Satofumi KAMIMURA

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

#include "UrgCtrl.h"
#include "UrgUtils.h"
#include "Connection.h"
#include "ConnectionUtils.h"
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
  bool adjustById(UrgCtrl urg[], long serials[], size_t n)
  {
    // シリアル ID を確保
    vector<long> device_serials;
    for (size_t i = 0; i < n; ++i) {
      long serial_id = urgSerialId<UrgCtrl>(&urg[i]);
      device_serials.push_back(serial_id);
    }

    // URG オブジェクトが シリアル ID で指定した順番になるようにする
    for (size_t i = 0; i < n; ++i) {
      long requested_serial = serials[i];
      if (device_serials[i] == requested_serial) {
        continue;
      }

      // 目標の ID があるインデックスを探して交換
      vector<long>::iterator p =
        find(device_serials.begin(), device_serials.end(), requested_serial);
      if (p == device_serials.end()) {
        return false;
      }
      size_t swap_index = p - device_serials.begin();

      swapConnection<UrgCtrl>(urg[i], urg[swap_index]);
      swap(device_serials[i], device_serials[swap_index]);
    }

    return true;
  }
}


int main(int argc, char *argv[])
{
  // 接続させたい URG の シリアル ID を順番に記述する
  long serials[] = {
    710002,
    614967,
  };
  const char* devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" };

  // URG オブジェクトの初期化
  UrgCtrl urg[2];

  // 接続
  size_t n = sizeof(serials) / sizeof(serials[0]);
  for (size_t i = 0; i < n; ++i) {
    if (! urg[i].connect(devices[i])) {
      cout << "UrgCtrl::connect: " << urg[i].what() << endl;
      exit(1);
    }
  }

  // 指定した シリアル ID とオブジェクトが一致するように調整
  if (! adjustById(urg, serials, n)) {
    cout << "fail swapById()" << endl;
    exit(1);
  }

  // 調整結果を表示
  cout << "result" << endl;
  for (size_t i = 0; i < n; ++i) {
    cout << i << ", " << urgSerialId<UrgCtrl>(&urg[i]) << endl;
  }

  // !!!

  for (size_t i = 0; i < n; ++i) {
    delete urg[i].connection();
    urg[i].setConnection(NULL);
  }

#ifdef MSC
  getchar();
#endif

  return 0;
}
