/*!
  \file
  \brief 柱の検出

  \author Satofumi KAMIMURA

  $Id: detectPoles.cpp 1566 2009-12-06 07:44:36Z satofumi $
*/

#include "detectPoles.h"
#include "RangeFinder.h"
#include "PointUtils.h"
#include "Rotate.h"
#include <cstdlib>
#include <limits>

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 == PolesDefaultLength) {
      min_length = lrf.minDistance();
    }
    if (max_length == PolesDefaultLength) {
      max_length = lrf.maxDistance();
    }
    length_range.first = min_length;
    length_range.last = max_length;
  }


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

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

    if (next_length < lrf_min) {
      next_length = numeric_limits<long>::min();
    }

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

    long diff = labs(edge_length - next_length);
    if (diff < PoleEdgeThreshold) {
      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(pole_t& pole, 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);

    pole.center = average_point;
    pole.radius = radius;

    return true;
  }
}


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

  long lrf_min = lrf.minDistance();

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

    pole_t pole;
    if (isValidRadius(pole, lrf, data, *it, max_radius_mm)) {
      // position 位置を加算する
      pole.center = rotate<long>(pole.center, position.angle);
      pole.center += Point<long>(position.x, position.y);
      poles.push_back(pole);
    }
  }
}

