/*!
  \file
  \brief 位置推定モジュールの動作サンプル

  \author Satofumi KAMIMURA

  $Id: obstacleAvoidance.cpp 1368 2009-10-04 05:04:44Z satofumi $
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "convert2d.h"
#include "SearchPath.h"
#include "FollowLines.h"
#include "MarkerManager.h"
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
  bool connectDevices(mBeegoDrive& run, mUrgDevice& urg)
  {
    if (! run.connect("/dev/usb/ttyUSB0")) {
      cout << "BeegoDrive::connect(): " << run.what() << endl;
      return false;
    }
    //run.setStraightVelocity(100);

    if (! urg.connect("/dev/ttyACM0")) {
      cout << "UrgDrive::connect(): " << urg.what() << endl;
      return false;
    }
    urg.setRequestMode(UrgDevice::GD_Capture);

    return true;
  }


  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;
  mUrgDevice urg;

  if (! connectDevices(run, urg)) {
    return 1;
  }

  MarkerManager marker;
  vector<long> data;

  // 経路探索用の領域を定義
  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();
  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) {
      // 200 [mm] の幅を持たせて障害物を登録する
      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 1
    // 障害物エリアの描画
    Color cyan(0.0, 1.0, 1.0);
    marker.drawPoints("obstacle", obstacle_points, cyan, RobotRadius * 2);
#endif
    marker.unlock();
  }

  run.stop();

  return 0;
}
