/*!
  \file
  \brief 位置推定モジュールの動作サンプル

  \author Satofumi KAMIMURA

  $Id: positioningSample.cpp 1447 2009-10-25 07:13:40Z satofumi $
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "LrfDirection.h"
#include "FeaturePointMap.h"
#include "splitLrfData.h"
#include "detectFeaturePoints.h"
#include "averagePoints.h"
#include "delay.h"
#include "ticks.h"
#include "MarkerManager.h"
#include <iostream>

using namespace qrk;
using namespace std;


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

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

    return true;
  }


  void plotFeaturePoints(MarkerManager& marker,
                         const vector<featurePoint_t>& feature_points)
  {
    if (feature_points.empty()) {
      return;
    }

    vector<Point<long> > points;
    vector<Point<long> > reflex_points;
    for (vector<featurePoint_t>::const_iterator it = feature_points.begin();
         it != feature_points.end(); ++it) {
      if (it->type == ReflexAngle) {
        reflex_points.push_back(it->point);
      } else {
        points.push_back(it->point);
      }
    }

    Color yellow(1.0, 1.0, 0.0);
    marker.drawPoints("feature_points", points, yellow, 50);

    Color cyan(0.0, 1.0, 1.0);
    marker.drawPoints("feature_points", reflex_points, cyan, 50);
  }


  void plotRobotPosition(MarkerManager& marker,
                         const Position<long>& position)
  {
    Color blue(0.0, 0.0, 1.0);
    marker.drawPoint("robot", Point<long>(position.x, position.y),
                     blue, 50, 300);
  }
}


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

  mBeegoDrive run;
  mUrgDevice urg;

  if (! connectDevices(run, urg)) {
    return 1;
  }

  MarkerManager marker;
  vector<long> data;

  LrfDirection urg_direction(&urg);
  FeaturePointMap urg_position;
  Position<long> position;

  int first_ticks = ticks();
  bool moved = false;

  while (true) {

    if ((! moved) && ((ticks() - first_ticks) > 1000)) {
      run.stopLine(1000, 0, deg(0));
      //run.stopLine(1000, 500, deg(30));
      moved = true;
    }

    urg.requestData();
    long timestamp = 0;
    urg.receiveData(data, &timestamp);

    // 向きの推定
    urg_direction.update(data);
    position.angle = urg_direction.direction();

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

    // 特徴点の抽出
    vector<range_t> points_group;
    splitLrfData(points_group, urg, data);

    for (vector<range_t>::iterator it = points_group.begin();
         it != points_group.end(); ++it) {
      distance_t distance;
      if (! averagePoints(distance, *it, data)) {
        continue;
      }

      vector<featurePoint_t> feature_points;
      vector<segment_t> segments;
      detectFeaturePoints(feature_points, segments,
                          position, urg, data, distance, *it);
      plotFeaturePoints(marker, feature_points);

      // 位置の推定
      urg_position.update(feature_points, position, timestamp);
    }

    // !!! run の位置を考慮した位置になるようにする
    // !!! それまでの位置の重みを 4 くらいにして平均位置を計算すればよい
    position = urg_position.position();

    plotRobotPosition(marker, position);
    marker.unlock();

    fprintf(stderr, "(%d, %d, %d)\n", position.x, position.y, position.angle.to_deg());
  }

  return 0;
}
