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

  \author Satofumi KAMIMURA

  $Id: path_follower.cpp 1452 2009-10-25 22:11:13Z satofumi $

  \todo 位置推定のモジュールを適用する
*/

#include <qrk_main.h>
#include <mUrgDevice.h>
#include <MarkerManager.h>
#include <LrfDirection.h>
#include <splitLrfData.h>
#include <detectPolls.h>
#include <detectLines.h>
#include <convert2d.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);

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

  MarkerManager marker;

  Position<long> position;
  LrfDirection urg_direction(&urg);
  vector<long> data;

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

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

    //plotLrfData(marker, urg, data);

    // 向き変位の推定
    urg_direction.update(data);
    plotOriginDirection(marker, urg_direction.direction());

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

    // 線分を検出
    for (vector<range_t>::const_iterator it = ranges.begin();
         it != ranges.end(); ++it) {
      vector<line_t> lines;
      detectLines(lines, urg, data, *it);
      plotLines(marker, lines);

      // 線分の傾きから向きを推定
      // !!!

      // 線分との距離から位置を推定
      // !!!

      // !!! バッファに登録する
      // !!! 1 sec 後に地図に登録する
    }

    // 柱の位置を抽出
    vector<featurePoint_t> polls;
    detectPolls(polls, urg, data, ranges);
    plotPolls(marker, polls);

    // !!! バッファに登録する
    // !!! 1 sec 後に地図に登録する

    // 点の変位から位置の推定
    // !!!

    marker.unlock();
  }

  return 0;
}
