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

  \author Satofumi KAMIMURA

  $Id: positioningSample.cpp 1501 2009-11-07 15:06:49Z satofumi $
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "LrfDirection.h"
#include "splitLrfData.h"
#include "detectPoles.h"
#include "ObjectMap.h"
#include "ObjectMapBuffer.h"
#include "ObjectMapUtils.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 plotPoles(MarkerManager& marker,
		 const vector<pole_t>& poles)
  {
    if (poles.empty()) {
      return;
    }

    vector<Point<long> > points;
    vector<Point<long> > reflex_points;
    for (vector<pole_t>::const_iterator it = poles.begin();
         it != poles.end(); ++it) {
      points.push_back(it->center);
    }

    Color yellow(1.0, 1.0, 0.0);
    marker.drawPoints("poles", points, yellow, 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);
  ObjectMap global_map;
  ObjectMapBuffer map_buffer;
  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> ranges;
    splitLrfData(ranges, urg, data);

    // 柱の位置を抽出
    vector<pole_t> poles;
    detectPoles(poles, urg, data, ranges, position);
    plotPoles(marker, poles);
    registerPoles(map_buffer, global_map, poles, timestamp);

    // 柱をつないだ直線から向きを推定
    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);

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

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

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

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

  return 0;
}
