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

  \author Satofumi KAMIMURA

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

#include "splitLrfData.h"
#include "RangeFinder.h"
#include <cstdlib>

#include <cstdio>

using namespace qrk;
using namespace std;


namespace
{
  void addGroup(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;

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

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

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

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

}
