/*!
  \brief 柱の検出

  \author Satofumi KAMIMURA

  $Id: detectPolls.cpp 1450 2009-10-25 21:15:53Z satofumi $
*/

#include "detectPolls.h"
#include "RangeFinder.h"
#include "PointUtils.h"

using namespace std;
using namespace qrk;


namespace
{
  void setLengthRange(range_t& length_range,
                      long& min_length, long& max_length,
                      const RangeFinder& lrf)
  {
    if (min_length == PollsDefaultLength) {
      min_length = lrf.minDistance();
    }
    if (max_length == PollsDefaultLength) {
      max_length = lrf.maxDistance();
    }
    length_range.first = min_length;
    length_range.last = max_length;
  }


  bool isPollEdge(int edge_index, int next_index, const vector<long>& data,
                  const range_t& length_range)
  {
    enum {
      PollEdgeThreshold = 600, // [mm]
    };

    long edge_length = data[edge_index];
    long next_length = data[next_index];

    if ((edge_length < length_range.first) ||
        (edge_length > length_range.last)) {
      return false;
    }

    long diff = labs(edge_length - next_length);
    if (diff < PollEdgeThreshold) {
      return false;
    }

    return true;
  }


  Point<long> averagePoint(const vector<Point<long> >& points)
  {
    Point<long> average;

    for (vector<Point<long> >::const_iterator it = points.begin();
         it != points.end(); ++it) {
      average += *it;
    }

    long n = points.size();
    return Point<long>(average.x / n, average.y / n);
  }


  bool isValidRadius(featurePoint_t& fp, const RangeFinder& lrf,
                     const vector<long>& data, const range_t& range,
                     int max_radius_mm)
  {
    Point<long> first_point =
      calculateXy<long>(data[range.first], lrf.index2rad(range.first));
    Point<long> last_point =
      calculateXy<long>(data[range.last], lrf.index2rad(range.last));

    long arc_width = length(first_point, last_point);
    if ((arc_width / 2) > max_radius_mm) {
      return false;
    }

    // 円の中心位置と半径の計算
    vector<Point<long> > points;
    points.push_back(first_point);
    points.push_back(last_point);

    // 半径の長さだけデータ長にオフセット値を加える
    const long radius = static_cast<long>(arc_width * 2 / 3);
    for (long i = range.first + 1; i < (range.last - 1); ++i) {
      points.push_back(calculateXy<long>(data[i] + radius,
                                         lrf.index2rad(i)));
    }
    Point<long> average_point = averagePoint(points);

    fp.type = Poll;
    fp.radius = radius;
    fp.point = average_point;

    return true;
  }
}


void qrk::detectPolls(std::vector<featurePoint_t>& polls,
                      const RangeFinder& lrf,
                      const std::vector<long>& data,
                      const std::vector<range_t>& ranges,
                      long max_radius_mm, long min_length, long max_length)
{
  // 有効なデータの範囲の作成
  range_t length_range;
  setLengthRange(length_range, min_length, max_length, lrf);

  // 隣の位置の点が、エラー値、または前後と 1000 [mm] 以上離れており、
  // 物体の幅が 600 mm 以内の幅であれば、柱とみなして処理する
  for (vector<range_t>::const_iterator it = ranges.begin();
       it != ranges.end(); ++it) {
    if ((! isPollEdge(it->first, it->first - 1, data, length_range)) ||
        (! isPollEdge(it->last, it->last + 1, data, length_range))) {
      continue;
    }

    featurePoint_t fp;
    if (isValidRadius(fp, lrf, data, *it, max_radius_mm)) {
      polls.push_back(fp);
    }
  }
}

