/*!
  \file
  \brief 経路ログの取得

  \author Satofumi KAMIMURA

  $Id: path_recorder.cpp 1517 2009-11-15 15:24:14Z satofumi $

  \todo 複数の取得データをまとめて１つの地図が生成できるようにする
*/

#include <qrk_main.h>
#include <mBeegoDrive.h>
#include <mUrgDevice.h>
#include <mCameraDevice.h>
#include <ticks.h>

using namespace qrk;
using namespace std;

//#define NO_BEEGO_DRIVE
#define NO_CAMERA


int main(int argc, char *argv[])
{
  static_cast<void>(argc);
  static_cast<void>(argv);

#if not defined(NO_BEEGO_DRIVE)
  mBeegoDrive run;
  if (! run.connect("/dev/ttyUSB0")) {
    cout << "BeegoDrive::connect: " << run.what() << endl;
    return 1;
  }
  run.freeServo();
#endif

  mUrgDevice urg;
  if (! urg.connect("/dev/ttyACM0")) {
    cout << "UrgDevice::connect: " << urg.what() << endl;
    return 1;
  }
  urg.setSkipLines(2);

#ifndef NO_CAMERA
  mCameraDevice camera;
  if (! camera.connect()) {
    cout << "CameraDevice::connect: " << camera.what() << endl;
    return 1;
  }
#endif

  enum { CameraCaptureInterval = 3000 };
  int first_ticks = -CameraCaptureInterval;

  size_t capture_index = 0;
  vector<long> data;
  while (true) {
    // URG データの記録
    urg.requestData();
    int n = urg.receiveData(data);
    if (n <= 0) {
      break;
    }

#if not defined(NO_BEEGO_DRIVE)
    // 走行コントローラの位置記録
    Position<long> run_position = run.position();
    cout << "(" << run_position.x << ", " << run_position.y << ") ";
#endif

    cout << ".";

    int current_ticks = ticks();
    if ((current_ticks - first_ticks) > CameraCaptureInterval) {
      first_ticks = current_ticks;

#ifdef NO_CAMERA
      (void)capture_index;
      cout << endl;
#else

      // カメラ画像の記録
      enum { BufferSize = 128 };
      char file_name[BufferSize];
      snprintf(file_name, BufferSize, "camera_%04d.jpg", capture_index);
      camera.capture(file_name);
      ++capture_index;

      cout << current_ticks << " [msec] (" << file_name << ")" << endl;
#endif
    }
  }

  return 0;
}
