/*!
  \file
  \brief 位置補正の動作サンプル

  \author Satofumi KAMIMURA

  $Id: positionOffset_sample.cpp 1585 2009-12-26 22:02:06Z satofumi $
*/

#include <qrk_main.h>
#include <mBeegoDrive.h>
#include <FollowLines.h>
#include <Rotate.h>
#include <connectDevice.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 (! connectDevice(run, "/dev/ttyUSB0")) {
    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>(1000, 1000));
#if 0
  points.push_back(Point<long>(0, 1000));
  points.push_back(Point<long>(0, 100));
#endif

  enum { OffsetDegree = 45 };

  // 傾けた経路を描画するための処理
  vector<Point<long> > rotated_points;
  for (vector<Point<long> >::iterator it = points.begin();
       it != points.end(); ++it) {
    rotated_points.push_back(rotate<long>(*it, deg(-OffsetDegree)));
  }

  MarkerManager marker;
  marker.lock();
  Color purple(1.0, 0.0, 1.0);
  marker.drawLines("path", points, purple, 50);

  Color blue(0.0, 0.0, 1.0);
  marker.drawLines("rotated_path", rotated_points, blue, 50);
  marker.unlock();

#if 1
  // 回転
  run.changeRobotPosition(Position<long>(0, 0, deg(OffsetDegree)),
                          Position<long>(0, 0, deg(0)));
#else
  // 並行移動
  run.changeRobotPosition(Position<long>(100, 100, deg(0)),
                          Position<long>(0, 0, deg(0)));
#endif

  FollowLines follow_lines(points, run);
  while (follow_lines.run()) {
    delay(100);
    Position<long> position = run.position();
    printf("%ld, %ld, # %d\n", position.x, position.y, position.to_deg());
  }
  run.stop();

  return 0;
}
