/*!
  \file
  \brief URG を用いた向き推定

  \author Satofumi KAMIMURA

  $Id: LrfDirection.cpp 1496 2009-11-03 13:44:02Z satofumi $
*/

#include "LrfDirection.h"
#include "RangeFinder.h"
#include "Angle.h"
#include <limits>
#include <cstdlib>

using namespace qrk;
using namespace std;


struct LrfDirection::pImpl
{
  vector<long> previous_data_;
  long resolution_max_;
  long direction_;
  int search_width_;
  int previous_offset_;


  pImpl(RangeFinder* lrf) : direction_(0), previous_offset_(0)
  {
    // 前方 90 度の範囲の index 幅を取得し、その値から角度分解能を計算する
    int index_minus45 = lrf->rad2index(-M_PI / 4.0);
    int index_plus45 = lrf->rad2index(+M_PI / 4.0);
    resolution_max_ = abs(index_plus45 - index_minus45) * 4;

    setSearchAngle(deg(18));
  }


  void setSearchAngle(const Angle& angle)
  {
    search_width_ =
      static_cast<long>(resolution_max_ * angle.to_rad() / (2.0 * M_PI));
  }


  int changedDirection(const vector<long>& current)
  {
    // !!! 高速化が必要ならば、極値がないことが確認した上で
    // !!! 探索をより評価値が高い方のみに行うように変更する

    // 前回の変位を覚えておき、その変位だけずらした位置から推定を行う
    int current_search_width = previous_offset_ + search_width_;

    // 探索範囲の計算
    int compare_last = current.size() - current_search_width;
    int compare_first = current_search_width;
    if (compare_last <= compare_first) {
      return 0;
    }

    // 最も距離の差が小さいときの index 値のずれを返す
    size_t min_value = numeric_limits<size_t>::max();
    int min_offset = 0;

    int n = current_search_width * 2;
    if (n > static_cast<int>(previous_data_.size())) {
      return 0;
    }

    for (int j = 0; j < n; ++j) {
      size_t value = 0;
      int offset_index = j - compare_first + previous_offset_;
      for (int i = compare_first; i < compare_last; ++i) {
        // !!! 高速化のため、ポインタ越しに比較したときの結果を評価してみる
        value += abs(current[i] - previous_data_[i + offset_index]);
      }
      if (value < min_value) {
        min_offset = offset_index;
        min_value = value;
      }
    }
    previous_offset_ = min_offset;
    return min_offset;
  }
};


LrfDirection::LrfDirection(RangeFinder* lrf) : pimpl(new pImpl(lrf))
{
}


LrfDirection::~LrfDirection(void)
{
}


void LrfDirection::setSearchAngle(const Angle& angle)
{
  pimpl->setSearchAngle(angle);
}


void LrfDirection::update(const std::vector<long>& data)
{
  if (! pimpl->previous_data_.empty()) {
    pimpl->direction_ += pimpl->changedDirection(data);
  }
  pimpl->previous_data_ = data;
}


Angle LrfDirection::direction(void) const
{
  // URG の分解能で角度を保持しておき、Angle に変換して返す
  double radian = 2.0 * M_PI * pimpl->direction_ / pimpl->resolution_max_;
  return rad(radian);
}


void LrfDirection::setDirection(const Angle& angle)
{
  double radian = angle.to_rad();
  pimpl->direction_ =
    static_cast<long>(pimpl->resolution_max_ * radian / (2.0 * M_PI));
}
