/*!
  \file
  \brief 柱を用いた位置推定

  \author Satofumi KAMIMURA

  $Id: positioning_poles.cpp 1584 2009-12-25 23:25:11Z satofumi $
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "connectDevice.h"
#include "loadData.h"
#include "FollowLines.h"
#include "splitLrfData.h"
#include "PointUtils.h"
#include "AngleUtils.h"
#include "detectPoles.h"
#include "plot_objects.h"
#include "MarkerManager.h"

using namespace qrk;
using namespace std;


namespace
{
  void drawMapPath(MarkerManager& marker, const vector<Point<long> >& map_path)
  {
    marker.lock();
    Color purple(1.0, 0.0, 1.0);
    marker.drawLines("map_path", map_path, purple, 25);
    marker.unlock();
  }


  void poleMatching(map<int, int>& matched_id,
                    const vector<pole_t>& map_poles,
                    const vector<pole_t>& poles)
  {
    enum {
      NearThreshold = 300,      // [mm]
    };

    int map_index = 0;
    for (vector<pole_t>::const_iterator map_it = map_poles.begin();
         map_it != map_poles.end(); ++map_it, ++map_index) {

      int index = 0;
      for (vector<pole_t>::const_iterator it = poles.begin();
           it != poles.end(); ++it, ++index) {
        if (length<long>(map_it->center, it->center) < NearThreshold) {
          matched_id[map_index] = index;
          break;
        }
      }
    }
  }
}


int main(int argc, char *argv[])
{
  static_cast<void>(argc);
  static_cast<void>(argv);

  // デバイスに接続
  mBeegoDrive run;
  mUrgDevice urg;
  if ((! connectDevice(run, "/dev/ttyUSB0")) ||
      (! connectDevice(urg, "/dev/ttyACM0"))) {
    return 1;
  }

  // データの読み出し
  vector<Point<long> > map_path;
  vector<pole_t> map_poles;
  const char* run_path_file = "run_path.txt";
  if ((! loadPath(map_path, run_path_file)) ||
      (! loadPoles(map_poles, "poles.txt"))) {
    return 1;
  }

  // コースの描画
  MarkerManager marker;
  drawMapPath(marker, map_path);

  // 何周もコースを走行させるために、コースデータを追加で読み込む
  enum { AdditionalRound = 2 };
  for (int i = 0; i < AdditionalRound; ++i) {
    loadPath(map_path, run_path_file);
  }

  vector<long> data;
  FollowLines follow_lines(map_path, run);
  while (follow_lines.run()) {
    fprintf(stderr, ".");

    // データ取得
    urg.requestData();
    urg.receiveData(data);
    Position<long> run_position = run.position();

    // URG データの分割
    vector<range_t> ranges;
    splitLrfData(ranges, urg, data);

    // 円柱の検出
    vector<pole_t> detected_poles;
    detectPoles(detected_poles, urg, data, ranges, run_position);

    // ロボットが定常状態で走行していて、
    // ２本以上の円柱が検出できた場合のみ、ロボット位置の推定と補正を行う
    if (run.isStable() && (detected_poles.size() >= 2)) {
      // 環境上の円柱と地図上の円柱とのマッチングを取る

      // 近しい円柱とのマッチングを取る
      // !!! 今は、柱が近いかどうかでマッチングの判定をしているが、
      // !!! 可能ならば、検出した柱の相対関係と地図上の柱の相対関係で
      // !!! マッチングを取るようにする
      // !!! そうすると、ロボットの向きのずれ、位置のずれに対してロバストになる
      map<int, int> matched_id;
      poleMatching(matched_id, map_poles, detected_poles);
      if (matched_id.size() == 2) {

        // !!! 円柱の相対関係でマッチングを判定するときに、
        // !!! 円柱のマッチングをどう取るかは、未定
        // !!! 円柱を順番に結んだときの直線で処理するのは、微妙
        // !!! 全ての円柱の間に直線を作るのは、直線が多くなりすぎそう
        // !!! 実装方法、未定

        // 円柱を結んだ直線の傾きから、ロボット向きの補正を行う
        // !!! 地図上のポールから向きを計算する
        // !!! 取得データのポールから向きを計算する
        Angle map_angle =
          rad(atan2(map_poles[0].center.y - map_poles[1].center.y,
                    map_poles[0].center.x - map_poles[1].center.x));

        int id_0 = matched_id[0];
        int id_1 = matched_id[1];
        Angle detected_angle = rad(atan2(detected_poles[id_0].center.y
                                         - detected_poles[id_1].center.y,
                                         detected_poles[id_0].center.x
                                         - detected_poles[id_1].center.x));
        //fprintf(stderr, "(%d, %d), ", map_angle.to_deg(), detected_angle.to_deg());

        Angle angle_diff = narrowAngles(map_angle, detected_angle);
        //fprintf(stderr, "%d, ", angle_diff.to_deg());

        // 円柱の位置から、ロボット位置の補正を行う
        Angle adjust_angle = rad(angle_diff.to_rad() * 0.1);

        // !!! changeRobotPosition() を使うように修正する
        //run.setRobotPositionOffset(Position<long>(0, 0, adjust_angle));

        // 円柱の位置から、ロボットの位置を修正する
        Point<long> point_diff;
        for (int i = 0; i < 2; ++i) {
          point_diff +=
            map_poles[i].center - detected_poles[matched_id[i]].center;
        }
        point_diff.x /= 2;
        point_diff.y /= 2;

        // 位置の補正
        Point<long> adjust_point(static_cast<long>(point_diff.x * 0.1),
                                 static_cast<long>(point_diff.y * 0.1));

        // !!! changeRobotPosition() を使うように修正する
        //run.setRobotPositionOffset(Position<long>(adjust_point.x,
        //adjust_point.y, deg(0)));

        // 直前の走行コマンドを再発行する
        // !!! この機能は、走行制御システム内で処理するように変更する
        run.followLine(follow_lines.currentLine());
      }
    }
    Point<long> robot_point(run_position.x, run_position.y);

    Position<long> origin_position;
    marker.lock();
    marker.clear("poles");
    plotPoles(marker, detected_poles, origin_position, "poles");

    marker.clear("robot");
    Color red(1.0, 0.0, 0.0);
    marker.drawPoint("robot", robot_point, red, 300);

    marker.unlock();
  }
  run.stop();

  return 0;
}
