/*!
  \file
  \brief 障害物を検出して停止するサンプル

  \author Satofumi KAMIMURA

  $Id: stopToObstacle.cpp 1520 2009-11-16 12:35:30Z satofumi $
*/

#include "qrk_main.h"
#include "SimulationWindow.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "convert2d.h"
#include "delay.h"
#include "ticks.h"
#include "MarkerManager.h"
#include <QApplication>

using namespace qrk;
using namespace std;


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

  MarkerManager marker;

  mBeegoDrive run;
  if (! run.connect("/dev/ttyUSB0")) {
    cout << "BeegoDrive::connect(): " << run.what() << endl;
    return 1;
  }

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

  vector<long> data;
  long timestamp = 0;
  long min_distance = urg.minDistance();
  size_t front_index = urg.deg2index(0);

  run.followLine(Position<long>(0, 0, deg(0)));

  // 前方に障害物があれば停止する
  enum {
    SimulationSec = 5,        // [sec]
    StopDistance = 500,       // [mm]
  };
  long first_ticks = ticks();
  while ((ticks() - first_ticks) < SimulationSec * 1000) {
    urg.requestData();

    Position<long> position = run.position();

    if (urg.receiveData(data, &timestamp)) {
      long distance = data[front_index];
      fprintf(stderr, "%ld [mm]\n", distance);
      if ((distance > min_distance) && (distance < StopDistance)) {
        break;
      }

#if 0
      // デバッグ用の表示
      marker.clear("urg");
      vector<Point<long> > points;
      convert2d(points, urg, data, position);
      Color red(1.0, 0.0, 0.0);
      marker.drawPoints("urg", points, red, 8, 70);
#endif
    }
  }

  cout << "stop()" << endl;
  run.stop();

  delay(2000);

  return 0;
}
