/*!
  \file
  \brief 連続直線の追従サンプル

  \author Satofumi KAMIMURA

  $Id: followLines.cpp 1387 2009-10-09 09:04:43Z satofumi $
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "FollowLines.h"
#include "delay.h"
#include "MarkerManager.h"

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>(1000, 0));
  points.push_back(Point<long>(1500, 1000));
  points.push_back(Point<long>(0, 1000));
  points.push_back(Point<long>(0, 100));

  // 追従する直線を描画
  MarkerManager marker;
  Color purple(1.0, 0.0, 1.0);
  marker.lock();
  marker.drawLines("path", points, purple, 20);
  marker.unlock();

  FollowLines follow_lines(points, run);

  while (follow_lines.run()) {
    delay(100);
  }
  run.stop();

  delay(2000);

  return 0;
}
