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

  \author Satofumi KAMIMURA

  \todo シミュレーション実行のときに、ゴールで停止しないことがあるのを修正する

  $Id: obstacleAvoidance.cpp 1690 2010-02-13 02:07:59Z satofumi $
*/

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

using namespace qrk;
using namespace std;


namespace
{
  bool connectDevices(mBeegoDrive& run, mUrgDevice& urg)
  {
    if (! run.connect("/dev/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;
  urg.setRequestMode(UrgDevice::GD_Capture);

  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);

  //FILE* fout = fopen("path.txt", "w");

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

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

    //fprintf(stderr, "%d, ", ticks());

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

    //fprintf(stderr, "%d, ", ticks());

    // 検出した障害物の配置
    position = run.position();
    //fprintf(fout, "%ld\t%ld\t # %d\n", position.x, position.y, position.deg());
    //fprintf(stderr, "%d\n", position.deg());
    //fflush(fout);

    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) {
        long x = it->x;
        long y = it->y;
        search.setObstacle(x - RobotRadius, y + i);
        search.setObstacle(x + RobotRadius, y + i);
        search.setObstacle(x + i, y - RobotRadius);
        search.setObstacle(x + i, y + RobotRadius);
      }
    }

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

    vector<Point<long> > follow_points;
    if (! search.path(follow_points)) {
      // 経路が見つからなければ、停止する
      run.stop();
    } else {

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

    marker.lock();
    marker.clear();

#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

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

    marker.unlock();

    //fprintf(stderr, "\n");
  }

  run.stop();

  return 0;
}
