/*!
  \example wall_follower.cpp 壁沿い走行の動作サンプル

  \author Satofumi KAMIMURA

  $Id$

  \todo 旋回中に線分の角度が大きくずれるのを修正する
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "TimestampManager.h"
#include "TimestampedPosition.h"
#include "ticks.h"
#include "FollowLines.h"
#include "splitLrfData.h"
#include "detectLines.h"
#include "PointUtils.h"
#include "PositionUtils.h"
#include "plot_objects.h"
#include "MarkerManager.h"
#include <iostream>

using namespace qrk;
using namespace std;


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

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

    return true;
  }


  void registerWalls(vector<line_t>& wall_lines)
  {
    line_t line;

    line.begin = Point<long>(3000, 1000);
    line.direction = deg(+180);
    line.length = 4000;
    wall_lines.push_back(line);

    line.begin = Point<long>(3000, -3000);
    line.direction = deg(+90);
    line.length = 4000;
    wall_lines.push_back(line);
  }


  void regsiterPath(vector<Point<long> >& path_points)
  {
    path_points.push_back(Point<long>(0, 0));
    path_points.push_back(Point<long>(500, 1));
    path_points.push_back(Point<long>(1000, 1));
    path_points.push_back(Point<long>(1500, 1));

    path_points.push_back(Point<long>(2000, 0));
    path_points.push_back(Point<long>(2000, -500));
    path_points.push_back(Point<long>(2000, -1000));
    path_points.push_back(Point<long>(2000, -1500));

    path_points.push_back(Point<long>(2000, -2000));

    path_points.push_back(Point<long>(2000, -2001));
    path_points.push_back(Point<long>(2000, -1500));
    path_points.push_back(Point<long>(2000, -1000));
    path_points.push_back(Point<long>(2000, -500));

    path_points.push_back(Point<long>(2000, 0));
    path_points.push_back(Point<long>(1500, 0));
    path_points.push_back(Point<long>(1000, 0));
    path_points.push_back(Point<long>(500, 0));

    path_points.push_back(Point<long>(0, 0));
  }


  void mergeLines(vector<line_t>& merged_lines,
                  const vector<line_t>& lines)
  {
    if (lines.empty()) {
      return;
    }

    // 始点が前の線分の終点と近く、傾きがほぼ同じときに、線分を連結する
    enum {
      NearLength = 200,         // [mm]
      NearDegree = 8,
    };
    line_t pre_line = lines.front();
    line_t merged_line;
    bool no_merged_line = true;

    for (vector<line_t>::const_iterator it = lines.begin() + 1;
         it != lines.end(); ++it) {
      // 終点の位置を計算する
      double radian = pre_line.direction.to_rad();
      long x_offset = pre_line.length * cos(radian);
      long y_offset = pre_line.length * sin(radian);
      Point<long> pre_line_end =
        pre_line.begin + Point<long>(x_offset, y_offset);

      int degree_diff = pre_line.direction.to_deg() - it->direction.to_deg();
      if (degree_diff > 180) {
        degree_diff -= 360;
      } else if (degree_diff < -180) {
        degree_diff += 360;
      }

      if ((length<long>(it->begin, pre_line_end) < NearLength) &&
          (abs(degree_diff) < NearDegree)) {
        // 連結した直線を作る
        if (no_merged_line) {
          merged_line.begin = pre_line.begin;
          merged_line.length = pre_line.length;
        }

        radian = it->direction.to_rad();
        x_offset = it->length * cos(radian);
        y_offset = it->length * sin(radian);
        Point<long> line_end =
          it->begin + Point<long>(x_offset, y_offset);

        merged_line.direction =
          rad(atan2(line_end.y - merged_line.begin.y,
                    line_end.x - merged_line.begin.x));
        merged_line.length += it->length;
        no_merged_line = false;
      } else {
        // 直線が連結されていれば、登録する
        if (! no_merged_line) {
          merged_lines.push_back(merged_line);
        }
        no_merged_line = true;
      }
      pre_line = *it;
    }

    // 直線が連結されていれば、登録する
    if (! no_merged_line) {
      merged_lines.push_back(merged_line);
    }
  }
}


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

  // デバイスとの接続
  mBeegoDrive run;
  mUrgDevice urg;
  if (! connectDevices(run, urg)) {
    return 1;
  }
  urg.setSkipLines(2);

  MarkerManager marker;

  // 地図上の壁の位置を定義
  vector<line_t> wall_lines;
  registerWalls(wall_lines);

  // 走行経路を定義
  vector<Point<long> > path_points;
  regsiterPath(path_points);

  marker.lock();
  plotLines(marker, wall_lines, "wall");

  Color purple(1.0, 0.0, 1.0);
  marker.drawLines("path", path_points, purple);
  marker.unlock();

  // タイムスタンプの初期化
  TimestampManager timestamps;
  timestamps.add(&urg);
  timestamps.add(&run);
  timestamps.setTimestamp(ticks());
  TimestampedPosition timestamped_position;

  // 検出した壁を利用した補正処理を行いつつ、点列を走行する
  FollowLines follow_lines(path_points, run);
  vector<long> data;
  while (follow_lines.run()) {

    // データの取得
    // !!! URG と Beego のデータ取得を、どちらを先にすべきかを検討すべき
    urg.requestData();
    long urg_raw_timestamp = 0;
    urg.receiveData(data, &urg_raw_timestamp);

    Position<long> position = run.position();
    long run_timestamp = timestamps.timestamp(&run);
    timestamped_position.addPosition(position, run_timestamp);

    long urg_timestamp = timestamps.timestamp(&urg, urg_raw_timestamp);
    Position<long> urg_position = timestamped_position.position(urg_timestamp);
    Point<long> robot_point(urg_position.x, urg_position.y);

    marker.lock();
    marker.clear("robot");
    Color red(1.0, 0.0, 0.0);
    marker.drawPoint("robot", robot_point, red, 50);
    marker.unlock();

    if (! run.isStable()) {
      continue;
    }

    // 線分の検出
    vector<range_t> ranges;
    splitLrfData(ranges, urg, data);

    vector<line_t> lines;
    detectLines(lines, urg, data, ranges, urg_position);

    // 連続した傾きの線分を連結子、長い直線にする
    vector<line_t> merged_lines;
    mergeLines(merged_lines, lines);

    // 環境中の壁と近いか判定し、マッチングを行う
    // ロボットからの距離と角度がほぼ同じだったら、同じ直線とみなす
    vector<long> to_walls;
    for (vector<line_t>::const_iterator it = wall_lines.begin();
         it != wall_lines.end(); ++it) {
      Position<long> line_position(it->begin.x, it->begin.y,
                                   it->direction - deg(90));
      to_walls.push_back(lengthToLine<long>(robot_point, line_position));
    }

    vector<long> to_lines;
    for (vector<line_t>::const_iterator it = merged_lines.begin();
         it != merged_lines.end(); ++it) {
      Position<long> line_position(it->begin.x, it->begin.y,
                                   it->direction - deg(90));
      to_lines.push_back(lengthToLine<long>(robot_point, line_position));
    }

    int total_degree_diff = 0;
    Point<long> total_offset;
    int matched_n = 0;
    size_t wall_size = to_walls.size();
    size_t lines_size = to_lines.size();
    for (size_t i = 0; i < wall_size; ++i) {
      for (size_t j = 0; j < lines_size; ++j) {

        int length_diff = to_walls[i] - to_lines[j];

        int degree_diff =
          wall_lines[i].direction.to_deg() - merged_lines[j].direction.to_deg();
        if (degree_diff > 180) {
          degree_diff -= 360;
        } else if (degree_diff < -180) {
          degree_diff += 360;
        }

        enum {
          NearLineLength = 600,
          NearLineDegree = 30,
        };

        if ((abs(length_diff) < NearLineLength) &&
            (abs(degree_diff) < NearLineDegree)) {

          // 向きの修正と、直線に垂直な方向への自己位置修正の量を計算する
          ++matched_n;
          total_degree_diff += degree_diff;

          Point<long> offset =
            calculateXy<long>(length_diff,
                              (wall_lines[i].direction - deg(90)).to_rad());
          total_offset += offset;
        }
      }
    }

    marker.lock();

    marker.clear("lrf_data");
    marker.clear("lines");
    marker.clear("poles");
    marker.clear("origin_direction");
    //marker.clear("robot");

    //plotLines(marker, lines);
    plotLines(marker, merged_lines);

    marker.unlock();

    if (matched_n > 0) {
      const double adjust_ratio = 0.1;
      // !!! 現在位置と、補正後の位置を表示
      // !!!
      long x = total_offset.x / matched_n;
      long y = total_offset.y / matched_n;
      double adjusted_radian = M_PI / 180.0 * total_degree_diff / matched_n;
      Position<long>
        adjusted_offset(-x, -y, rad(adjust_ratio * adjusted_radian));
      Position<long> adjusted_position = position + adjusted_offset;

      run.changeRobotPosition(position, adjusted_position);
    }
  }
  run.stop();

  return 0;
}
