/*!
  \file
  \brief つくばチャレンジ 2009 用の試走会用の走行プログラム

  \author Satofumi KAMIMURA

  $Id: trial_2009.cpp 1540 2009-11-20 07:47:10Z satofumi $
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "loadData.h"
#include "PointUtils.h"
#include "PositionUtils.h"
#include "FollowLines.h"
#include "splitLrfData.h"
#include "detectLines.h"
#include "mergeLines.h"
#include "detectPoles.h"
#include "AngleUtils.h"
#include "MarkerManager.h"
#include "plot_objects.h"
#include "ticks.h"
#include <fstream>
#include <iostream>
#include <cstdlib>

using namespace qrk;
using namespace std;


namespace
{
  class ObstacleState
  {
  public:
    void clear(void)
    {
      first_ticks_ = ticks();
    }


    long ticks(void) const
    {
      return (qrk::ticks() - first_ticks_);
    }

  private:
    long first_ticks_;
  };

  ObstacleState obstacle_state_;


  class MatchedPole {
  public:
    int id;
    long length_diff;

    MatchedPole(void) : id(-1)
    {
    }
  };


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


  bool detectedObstacles(const mUrgDevice& urg,
                         const vector<long>& data, long obstacle_length)
  {
    int first_index = urg.deg2index(-45);
    int last_index = urg.deg2index(+45);
    int min_length = urg.minDistance();

    for (int i = first_index; i < last_index; i += 2) {
      long length = data[i];
      if (length <= min_length) {
        continue;
      }

      if (length < obstacle_length) {
        return true;
      }
    }

    return false;
  }


  bool avoidObstacles(bool detected, mBeegoDrive& run, mUrgDevice& urg,
                      const vector<long>& data, long obstacle_length)
  {
    if (! detected) {
      // 最初にこの関数に入ったときは、ロボットを停止させ状態を初期化する
      run.stop();
      obstacle_state_.clear();
      return false;
    }

    // 数秒間は、障害物がいなくなるのを待つ
    enum { WaitMsec = 50000 };
    if (obstacle_state_.ticks() < WaitMsec) {
      if (detectedObstacles(urg, data, obstacle_length)) {
        return false;
      }
      return true;
    }

    // 障害物がなくならなければ、A* による回避を行う
    // !!! delete search_path_
    // !!! SearchPath を作成
    // !!! スタートとゴールを設定
    // !!! これらの処理は、１回だけ行う
    // !!! search_path_ が NULL だったら、行うのでよい
    (void)urg;
    // !!!

    // A* の目標位置までに障害物がなければ、通常の経路追従に復帰する
    if (0) {
      return true;
    }

    return true;
  }


  void trial(mBeegoDrive& run, mUrgDevice& urg)
  {
    // 走行に必要なデータの読み出し
    vector<Point<long> > run_path;
    vector<line_t> wall_lines;
    vector<pole_t> map_poles;

    if ((! loadPath(run_path, "run_path.txt")) ||
        (! loadLines(wall_lines, "lines.txt")) ||
        (! loadPoles(map_poles, "poles.txt"))) {
      exit(1);
    }

    MarkerManager marker;

    marker.lock();
    plotLines(marker, wall_lines, "wall");
    Color purple(1.0, 0.0, 1.0);
    marker.drawLines("path", run_path, purple);
    marker.unlock();

    FollowLines follow_lines(run_path, run);
    vector<long> data;
    bool obstacles_detected = false;
    while (follow_lines.run()) {

      do {
        fprintf(stderr, ".");

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

#if 1
        marker.lock();
        marker.clear("lrf_data");
        plotLrfData(marker, urg, data, "lrf_data");
        marker.unlock();
#endif

        // 前方 90 deg の 600 mm 以内に障害物を検出したら、
        // 障害物を検出したときの処理を開始する
        enum { ObstacleLength = 600 };
        if (obstacles_detected ||
            detectedObstacles(urg, data, ObstacleLength)) {
          obstacles_detected = ! avoidObstacles(obstacles_detected, run, urg,
                                                data, ObstacleLength);
          if (! obstacles_detected) {
            // 障害物がなくなったら、走行を再開する
            run.followLine(follow_lines.currentLine());
          }
        }

        Point<long> robot_point(run_position.x, run_position.y);

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

        if (! run.isStable()) {
          // 直線追従が安定しているときのみ、位置補正を行う
          continue;
        }

#if 0
        if (run_position.x < 9000) {
          // 最初の 9 [m] は位置補正を行わない
          continue;
        }
#endif
        // URG データの分割
        vector<range_t> ranges;
        splitLrfData(ranges, urg, data);

#if 0
        // 直線を用いた位置補正
        vector<line_t> lines;
        detectLines(lines, urg, data, ranges, run_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;

              // !!! 壁の向きと、筐体の向きとの差を登録すべき
              //fprintf(stderr, "(%d), ", degree_diff);
              total_degree_diff += degree_diff;

#if 1
              // !!! 位置補正
              // !!! 今の方法だと、補正するたびにずれが発生する
              // !!! URG データと位置の同期を行っていないため
              Point<long> offset =
                calculateXy<long>(length_diff,
                                  (wall_lines[i].direction - deg(90)).to_rad());
              total_offset += offset;
#endif
            }
          }
        }
#endif

        marker.lock();

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

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

        marker.unlock();

#if 0
        // 柱を用いた位置補正
        vector<pole_t> poles;
        detectPoles(poles, urg, data, ranges, run_position);

        vector<long> to_detected_poles;
        for (vector<pole_t>::const_iterator it = poles.begin();
             it != poles.end(); ++it) {
          Point<long> pole_point(it->center.x, it->center.y);
          to_detected_poles.push_back(length<long>(pole_point, robot_point));
        }

        vector<long> to_map_poles;
        for (vector<pole_t>::const_iterator it = map_poles.begin();
             it != map_poles.end(); ++it) {
          Point<long> pole_point(it->center.x, it->center.y);
          to_map_poles.push_back(length<long>(pole_point, robot_point));
        }

        enum { LengthThreshold = 1000 };
        size_t map_size = to_map_poles.size();
        vector<MatchedPole> matched_poles;
        matched_poles.resize(map_size);
        size_t detected_size = to_detected_poles.size();
        for (size_t i = 0; i < map_size; ++i) {
          for (size_t j = 0; j < detected_size; ++j) {
            int length_diff = abs(to_map_poles[i] - to_detected_poles[j]);
            if (length_diff < LengthThreshold) {
              if (matched_poles[i].id < 0) {
                matched_poles[i].id = j;
                matched_poles[i].length_diff = length_diff;
              } else {
                if (length_diff < matched_poles[i].length_diff) {
                  matched_poles[i].id = j;
                  matched_poles[i].length_diff = length_diff;
                }
              }
            }
          }
        }

        // 対応している点との角度差を計算する
        int total_degree_diff = 0;
        int matched_n = 0;
        for (size_t i = 0; i < map_size; ++i) {
          int id = matched_poles[i].id;
          if (id >= 0) {
            Angle diff = narrowAngle<long>(map_poles[i].center, robot_point,
                                           poles[id].center);
            total_degree_diff += diff.to_deg();
            ++matched_n;
          }
        }

        // 位置ずれを計算する
        Point<long> total_offset;
#if 0
        for (size_t i = 0; i < map_size; ++i) {
          int id = matched_poles[i].id;
          if (id >= 0) {
            total_offset += map_poles[i].center - poles[id].center;
          }
        }
#endif

#endif

#if 0
        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 = run_position + adjusted_offset;
          //fprintf(stderr, "(%d, %d, %d), ", adjusted_position.x, adjusted_position.y, adjusted_position.deg());
          //fprintf(stderr, "(%ld, %ld, %d), ", adjusted_offset.x, adjusted_offset.y, adjusted_offset.deg());

          run.setRobotPositionOffset(adjusted_offset);
          fprintf(stderr, "x = %ld, ", run.position().x);
        }
#endif

      } while (obstacles_detected);
    }
  }
}


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);
  //run.setRobotPosition(Position<long>(8000, 0, deg(0)));

  trial(run, urg);
  run.stop();

  return 0;
}
