/*!
  \file
  \brief 経路の追従プログラム

  \author Satofumi KAMIMURA

  $Id: path_follower.cpp 1585 2009-12-26 22:02:06Z satofumi $
*/

#include <qrk_main.h>
#include <mBeegoDrive.h>
#include <mUrgDevice.h>
#include <MarkerManager.h>
#include <LrfDirection.h>
#include <splitLrfData.h>
#include <detectPoles.h>
#include <detectLines.h>
#include <ObjectMap.h>
#include <ObjectMapBuffer.h>
#include <ObjectMapUtils.h>
#include <iostream>
#include "plot_objects.h"

using namespace qrk;
using namespace std;


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

#if not defined(NO_BEEGO_DRIVE)
  mBeegoDrive run;
  if (! run.connect("/dev/ttyUSB0")) {
    cout << "BeegoDrive::connect: " << run.what() << endl;
    return 1;
  }
#endif

  mUrgDevice urg;
  if (! urg.connect("/dev/ttyACM0")) {
    cout << "UrgDevice::connect: " << urg.what() << endl;
    return 1;
  }
  urg.setSkipLines(2);

  MarkerManager marker;

  ObjectMap global_map;
  ObjectMapBuffer map_buffer;
  Position<long> position;
  LrfDirection urg_direction(&urg);
  vector<long> data;

  FILE* path_ = fopen("path.txt", "w");
  FILE* run_path_ = fopen("run_path.txt", "w");

  while (true) {
    // URG データの記録
    urg.requestData();
    long timestamp = 0;
    int n = urg.receiveData(data, &timestamp);
    if (n <= 0) {
      break;
    }

#if not defined(NO_BEEGO_DRIVE)
    // 走行コントローラの位置記録
    Position<long> run_position = run.position();
    //cout << "(" << run_position.x << ", " << run_position.y << ") ";
    fprintf(run_path_, "%ld\t%ld\t # %d\n", run_position.x, run_position.y, run_position.to_deg());
    fflush(run_path_);
#endif

//     // !!! 最初の４秒のデータは無視する
//     if (timestamp < 20000) {
//       continue;
//     }

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

    //plotLrfData(marker, urg, data);

    // 向き変位の推定
    urg_direction.update(data);
    const Angle base_direction = urg_direction.direction();
    plotOriginDirection(marker, base_direction);
    position.angle = base_direction;

    // 計測データを連続している領域毎に分割する
    vector<range_t> ranges;
    splitLrfData(ranges, urg, data);

    // 前後の移動距離を推定
    // !!! 10 mm のグリッドを作り、移動距離を推定する

    // 線分を検出
    vector<line_t> lines;
    detectLines(lines, urg, data, ranges, position);
    plotLines(marker, lines);

    // 線分の傾きから向きを推定
    // !!! base_direction を適用した上の微調整を行う
    // !!!

    // 線分との距離から位置を補正
    // !!! 補正距離をベクトルで返す

    // 移動物体かを判定するため、１度バッファに登録する
    registerLines(map_buffer, lines, timestamp);

    // 距離の補正を反映させる
    // !!!

    // 柱の位置を抽出
    vector<pole_t> poles;
    detectPoles(poles, urg, data, ranges, position, 400, 500);
    registerPoles(map_buffer, global_map, poles, timestamp);
    //fprintf(stderr, "\n");
    //fprintf(stderr, "# polls: %d, ", poles.size());

    // 柱をつないだ直線から向きを推定
    Angle poles_angle_diff =
      angleFromPoles(&global_map, map_buffer.front());
    Position<long> previous_position = position;
    position.angle += poles_angle_diff;
    position.angle.normalize();
    updateObjectPosition(map_buffer.front(), position, previous_position);

    // 柱の位置から位置を推定
    Point<long> poles_move =
      pointFromPoles(&global_map, map_buffer.front());
    previous_position = position;
    position.x += poles_move.x;
    position.y += poles_move.y;
    updateObjectPosition(map_buffer.front(), position, previous_position);

    plotPoles(marker, poles, position);

    // 最も過去のデータを global_map に追加する
    mergeStaticObject(global_map, map_buffer);

    // 補正後の位置を反映させる
    urg_direction.setDirection(position.angle);

    fprintf(path_, "%ld\t%ld\t# %d\n", position.x, position.y, position.to_deg());
    fflush(path_);

    //printf("%ld\t%ld\t# %d\n", position.x, position.y, position.deg());

    marker.unlock();
  }

  return 0;
}
