/*!
  \file
  \brief 特徴点の表示サンプル

  \author Satofumi KAMIMURA

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

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

using namespace qrk;
using namespace std;


namespace
{
  void plotPointsGroup(MarkerManager& marker, const RangeFinder& range_finder,
                       const vector<long>& data, const range_t& range)
  {
    vector<Point<long> > points;
    for (long i = range.first; i < range.last; ++i) {
      long length = data[i];
      double radian = range_finder.index2rad(i);
      long x = static_cast<long>(length * cos(radian));
      long y = static_cast<long>(length * sin(radian));

      points.push_back(Point<long>(x, y));
    }
    Color red(1.0, 0.0, 0.0);
    marker.drawPoints("original", points, red);
  }


  void plotMedianPoints(MarkerManager& marker, const RangeFinder& range_finder,
                        distance_t& distance)
  {
    vector<Point<long> > points;
    range_t& range = distance.range;
    int index = 0;
    for (long i = range.first; i < range.last; ++i, ++index) {
      long length = distance.data[index];
      double radian = range_finder.index2rad(i);
      long x = static_cast<long>(length * cos(radian));
      long y = static_cast<long>(length * sin(radian));

      points.push_back(Point<long>(x, y));
    }
    Color red(1.0, 0.0, 0.0);
    marker.drawPoints("median", points, red);
  }


  void plotSegments(MarkerManager& marker,
                    const vector<segment_t>& segments)
  {
    if (segments.empty()) {
      return;
    }

    vector<Point<long> > points;
    for (vector<segment_t>::const_iterator it = segments.begin();
         it != segments.end(); ++it) {
      points.push_back(it->begin);
    }

    // 点列の最後の点を登録
    double length = segments.back().length;
    double radian = segments.back().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 purple(1.0, 0.0, 1.0);
    marker.drawLines("segments", points, purple);
  }


  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);
  }
}


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

  MarkerManager marker;

  mUrgDevice urg;
  if (! urg.connect("/dev/ttyACM0")) {
    cout << "UrgDrive::connect(): " << urg.what() << endl;
    return 1;
  }
  urg.setRequestMode(UrgDevice::GD_Capture);
  vector<long> data;

  Position<long> position;
  while (true) {
    urg.requestData();
    urg.receiveData(data);

    // 連続したとみなす点列グループを作成する
    vector<range_t> points_group;
    splitLrfData(points_group, urg, data);

    marker.lock();
    marker.clear("original");
    marker.clear("median");
    marker.clear("features");
    marker.clear("segments");
    marker.clear("feature_points");

    for (vector<range_t>::iterator it = points_group.begin();
         it != points_group.end(); ++it) {
      //plotPointsGroup(marker, urg, data, *it);

      // データ点にフィルターを適用し、点列の振動を抑える
      distance_t distance;
      //if (! medianPoints(distance, *it, data)) {
      if (! averagePoints(distance, *it, data)) {
        // フィルターを適用することでデータ数がゼロになったら、以降の処理を行わない
        continue;
      }
      //plotMedianPoints(marker, urg, distance);

      // 特徴点を抽出する
      vector<featurePoint_t> feature_points;
      vector<segment_t> segments;
      detectFeaturePoints(feature_points, segments,
                          position, urg, data, distance, *it);
      plotSegments(marker, segments);
      plotFeaturePoints(marker, feature_points);
    }
    marker.unlock();
    //fprintf(stderr, "\n");
  }
  return 0;
}
