/*!
  \file
  \brief LRF データの分割

  \author Satofumi KAMIMURA

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

#include "splitLrfData.h"
#include "RangeFinder.h"

using namespace qrk;
using namespace std;


namespace
{
  void addGrouup(vector<range_t>& points_group,
                 size_t begin_index, size_t index)
  {
    range_t group;
    group.first = begin_index;
    group.last = index;
    points_group.push_back(group);
  }
}


void qrk::splitLrfData(std::vector<range_t>& ranges,
                       const RangeFinder& lrf,
                       std::vector<long>& data, long threshold_mm)
{
  // 最初と最後の点は登録しないため、点が２つ以下だったら、戻る
  if (data.size() <= 2) {
    return;
  }

  long min_distance = lrf.minDistance();
  long max_distance = lrf.maxDistance();

  long pre_distance = 0;
  size_t begin_index = 0;
  size_t index = 0;

  vector<long>::const_iterator end_it = data.end() - 1;
  for (vector<long>::const_iterator it = data.begin() + 1;
       it != end_it; ++it, ++index) {
    long distance = *it;

    // 点列の切れ目が見つかれば、そこまでのグループを登録
    if ((distance <= min_distance) || (distance >= max_distance) ||
        (abs(distance - pre_distance) > threshold_mm)) {

      // グループの要素数が、２以上であれば登録
      if ((index - begin_index) >= 2) {
        addGrouup(ranges, begin_index, index);
      }
      begin_index = index + 1;
    }
    pre_distance = distance;
  }

  if ((index - begin_index) >= 2) {
    addGrouup(ranges, begin_index, index);
  }
}
