/*!
  \example cpp/move_astar.cpp

  \brief 「A* アルゴリズムを用いた障害物回避」サンプル

  \author Satofumi KAMIMURA

  $Id: move_astar.cpp 1688 2010-02-11 23:10:16Z satofumi $
*/

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

using namespace qrk;
using namespace std;


namespace
{
  long length(const Position<long>& position, const Point<long>& point)
  {
    long x = position.x - point.x;
    long y = position.y - point.y;
    return static_cast<long>(sqrt((x * x) + (y * y)));
  }
}


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;
  }
  mUrgDevice urg;
  if (! urg.connect("/dev/ttyACM0")) {
    cout << "UrgDevice::connect: " << urg.what() << endl;
    return 1;
  }

  MarkerManager marker;

  // 経路探索に使う領域を設定
  const long GridLength = 100;  // [mm]
  SearchPath search(60, 60);
  search.setOrigin(1, 30);
  search.setMagnify(GridLength);

  // 移動先を設定
  Point<long> goal(4000, 0);
  search.setGoal(goal.x, goal.y);

  Position<long> position = run.position();
  vector<long> data;
  while (length(position, goal) > 100) {

    // 毎回、障害物の情報をクリアしても、
    // 周囲環境が複雑でなければ、動作にあまり影響はないように思う
    search.clear();

    // 周囲環境データの取得
    urg.requestData();
    urg.receiveData(data);

    // 検出した障害物の配置
    position = run.position();
    vector<Point<long> > obstacle_points;
    convert2d(obstacle_points, urg, data, position);
    const long RobotRadius = 230; // [mm]
    for (vector<Point<long> >::iterator it = obstacle_points.begin();
         it != obstacle_points.end(); ++it) {
      // 幅を持たせて障害物を登録する
      for (int i = -RobotRadius; i < RobotRadius; i += GridLength) {
        search.setObstacle(it->x - RobotRadius, it->y + i);
        search.setObstacle(it->x + RobotRadius, it->y + i);
        search.setObstacle(it->x + i, it->y - RobotRadius);
        search.setObstacle(it->x + i, it->y + RobotRadius);
      }
    }

    // 経路の導出
    search.setStart(position.x, position.y);

    vector<Point<long> > follow_points;
    search.path(follow_points);

    // 経路追従
    FollowLines follow(follow_points, run);
    follow.run();

    marker.lock();
    marker.clear("path");
    marker.clear("obstacle");
#if 1
    // 経路の描画

    Color purple(1.0, 0.0, 1.0);
    marker.drawLines("path", follow_points, purple, 30);
#endif

#if 0
    // 障害物エリアの描画
    Color cyan(0.0, 1.0, 1.0);
    marker.drawPoints("obstacle", obstacle_points, cyan, RobotRadius * 2);
#endif
    marker.unlock();
  }

  run.stop();

  return 0;
}
