/*!
  \example タイムスタンプを用いた URG による環境測定

  \author Satofumi KAMIMURA

  $Id$
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "TimestampManager.h"
#include "TimestampedPosition.h"
#include "splitLrfData.h"
#include "detectLines.h"
#include "ticks.h"
#include "MarkerManager.h"
#include <convert2d.h>
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
  enum {
    MarkerWidth = 50,
  };


  void plotLines(MarkerManager& marker, const vector<line_t>& lines)
  {
    if (lines.empty()) {
      return;
    }

    for (vector<line_t>::const_iterator it = lines.begin();
         it != lines.end(); ++it) {

      vector<Point<long> > points;
      points.push_back(it->begin);

      // 点列の最後の点を登録
      double length = it->length;
      double radian = it->direction.to_rad();
      Point<long> offset(static_cast<long>(length * cos(radian)),
                         static_cast<long>(length * sin(radian)));
      points.push_back(points.back() + offset);

      Color green(0.0, 1.0, 0.0);
      marker.drawLines("lines", points, green, MarkerWidth);
    }
  }
}


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

  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;
  }
  urg.setSkipLines(2);

  MarkerManager marker;

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

  // 計測開始
  long first_ticks = ticks();
  bool moved = false;
  vector<long> data;

  while (true) {

    // データの取得
    urg.requestData();
    long urg_raw_timestamp = 0;
    urg.receiveData(data, &urg_raw_timestamp);

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

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

    vector<line_t> lines;
    long urg_timestamp = timestamps.timestamp(&urg, urg_raw_timestamp);
    Position<long> urg_position = timestamped_position.position(urg_timestamp);
    detectLines(lines, urg, data, ranges, urg_position);

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

    plotLines(marker, lines);
    marker.unlock();

    if ((! moved) && ((ticks() - first_ticks) > 1000)) {
      run.rotate(deg(120));
      moved = true;
    }

    if (moved) {
      if (run.isStable()) {
        break;
      }
    }
  }

  return 0;
}
