/*!
  \example cpp/move_lines.cpp

  \brief 「点列に沿って走る」サンプル

  \author Satofumi KAMIMURA

  $Id: move_lines.cpp 1407 2009-10-13 14:04:05Z satofumi $
*/

#include <qrk_main.h>
#include <mBeegoDrive.h>
#include <FollowLines.h>
#include <MarkerManager.h>
#include <delay.h>
#include <vector>

using namespace qrk;
using namespace std;


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

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

  // 走行経路の定義
  vector<Point<long> > points;
  points.push_back(Point<long>(0, 0));
  points.push_back(Point<long>(500, 0));
  points.push_back(Point<long>(1000, 1000));
  points.push_back(Point<long>(1500, 0));
  points.push_back(Point<long>(2000, 0));
  points.push_back(Point<long>(2000, -500));
  points.push_back(Point<long>(1000, -500));
  points.push_back(Point<long>(500, +1000));
  points.push_back(Point<long>(0, +100));

  // 走行経路の描画
  MarkerManager marker;
  Color cyan(1.0, 0.0, 1.0);
  marker.lock();
  marker.drawLines("path", points, cyan, 20);
  marker.unlock();

  // 経路追従
  FollowLines follow_lines(points, run);
  while (follow_lines.run()) {
    delay(100);
  }
  run.stop();

  delay(2000);

  return 0;
}
