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

  \author Satofumi KAMIMURA

  $Id: followLines.cpp 1534 2009-11-18 09:32:05Z 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/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()) {

    //marker.lock();
    marker.clear("robot");

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

    // ロボット位置の描画
    Color red(1.0, 0.0, 0.0);
    //fprintf(stderr, "%ld, %ld\n", position.x, position.y);
    Point<long> robot_point(position.x, position.y);
    marker.drawPoint("robot", robot_point, red, 50);

    //marker.unlock();

    delay(100);
  }
  run.stop();

  delay(2000);

  return 0;
}
