/*!
  \file
  \brief 特徴点から構成されるマップ

  \author Satofumi KAMIMURA

  $Id: FeaturePointMap.cpp 1447 2009-10-25 07:13:40Z satofumi $
*/

#include "FeaturePointMap.h"
#include "PointMap.h"
#include "PointUtils.h"
#include <deque>

using namespace qrk;
using namespace std;


namespace
{
  enum {
    DefaultPeriodMsec = 1000,
  };


  class Buffer
  {
  public:
    PointMap maps[Unknown];
    long timestamp;


    Buffer(void) : timestamp(0)
    {
    }
  };
}


struct FeaturePointMap::pImpl
{
  Position<long> position_;
  deque<Buffer*> buffers_;
  long period_msec_;
  Buffer global_map_;
  size_t id_;

  Point<long> offset_total_;
  int offset_total_size_;


  pImpl(void) : period_msec_(DefaultPeriodMsec), id_(1), offset_total_size_(0)
  {
    adjustBufferSize();
  }


  void adjustBufferSize(void)
  {
    if (buffers_.empty()) {
      // バッファが空の場合、追加
      Buffer* buffer = new Buffer;
      buffers_.push_front(buffer);

    } else {
      // 余分なバッファを削除
      long first_timestamp = buffers_.front()->timestamp;
      while ((buffers_.back()->timestamp - first_timestamp) > period_msec_) {
        delete buffers_.back();
        buffers_.pop_back();
      }
    }
  }


  void matching(size_t buffer_index, const featurePoint_t& feature_point)
  {
    const Point<long>& point = feature_point.point;
    FeaturePointType type = feature_point.type;
    //fprintf(stderr, "<%d, %d>, ", point.x, point.y);

    PointMap& point_map = buffers_[buffer_index]->maps[type];

    // buffer_index の次から buffers_ の最後までを処理対象とする
    enum { MatchingSize = 6 };
    int n = min(buffers_.size() - buffer_index,
                static_cast<size_t>(MatchingSize));
    for (int i = 0; i < n; ++i) {
      size_t index = buffer_index + i;

      vector<size_t> ids;
      // !!! 固定値をマクロにする
      buffers_[index]->maps[type].nearPoints(ids, point, 99);
      if (ids.size() != 1) {
        // 特徴点が近くに複数あった場合は登録しない
        continue;
      }

      // 過去のバッファと同じ位置に特徴点があれば、その ID を用いる
      //fprintf(stderr, "(marged %d), ", ids[0]);
      point_map.marge(ids[0], point);
      return;
    }

    ++id_;
    point_map.add(point, id_);
    //fprintf(stderr, "(add %d), ", id_);
  }


  void calculateMoveOffset(const vector<size_t>& no_move_ids, size_t type)
  {
    PointMap& point_map = buffers_.front()->maps[type];
    for (vector<size_t>::const_iterator it = no_move_ids.begin();
         it != no_move_ids.end(); ++it) {
      // 特徴点の移動幅を計算する
      Point<long> point = point_map.point(*it);
      if (global_map_.maps[type].exists(*it)) {
        Point<long> map_point = global_map_.maps[type].point(*it);
        offset_total_ += map_point - point;
        //fprintf(stderr, "(%d, %d), ", point.x, point.y);
        ++offset_total_size_;
      }
    }
  }


  void margePoints(const vector<size_t>& no_move_ids, size_t type,
                   const Point<long>& move_offset)
  {
    PointMap& point_map = buffers_.back()->maps[type];
    for (vector<size_t>::const_iterator it = no_move_ids.begin();
         it != no_move_ids.end(); ++it) {
      // 特徴点を登録する
      Point<long> point = point_map.point(*it) + move_offset;
      global_map_.maps[type].marge(*it, point);
    }
  }


  void update(const vector<featurePoint_t>& feature_points,
              const Position<long>& sensor_position,
              long timestamp)
  {
    (void)sensor_position;

    bool added = false;

    size_t buffer_index = 0;
    long first_timestamp = buffers_.front()->timestamp;
    long last_timestamp = buffers_.back()->timestamp;
    if (timestamp < last_timestamp) {
      // バッファ最後のタイムスタンプよりも古いデータならば、無視
      return;

    } else if (timestamp > first_timestamp) {

      // 移動していない点を地図に登録する
      registerStaticPoint();

      // 最古のバッファを削除
      if ((timestamp - last_timestamp) > period_msec_) {
        delete buffers_.back();
        buffers_.pop_back();
      }

      // 新しくバッファを作成して登録
      Buffer* buffer = new Buffer;
      buffer->timestamp = timestamp;
      buffers_.push_front(buffer);
      buffer_index = 0;

      added = true;
    } else {
      // 既に存在するバッファから、位置を検索
      while ((buffer_index < buffers_.size()) &&
             (timestamp <= buffers_[buffer_index]->timestamp)) {
        ++buffer_index;
      }
      --buffer_index;
    }

    // 特徴点の登録
    for (vector<featurePoint_t>::const_iterator it = feature_points.begin();
         it != feature_points.end(); ++it) {
      FeaturePointType type = it->type;
      if (type != Unknown) {
        // 前のバッファにさかのぼって特徴点のマッチングを行い
        // 特徴点の登録を行う
        matching(buffer_index, *it);
      }
    }
  }


  void registerStaticPoint(void)
  {
    if (buffers_.back()->timestamp <= 0) {
      // 空のデータの場合は、処理を行わない
      return;
    }

    // 最古のバッファと最新のバッファの特徴点の位置を比較し、
    // 移動してない特徴点ならば、マップに登録する
    vector<size_t> no_move_ids[Unknown];
    for (size_t type = 0; type < Unknown; ++type) {
      PointMap& front_buffer = buffers_.front()->maps[type];
      PointMap& back_buffer = buffers_.back()->maps[type];

      vector<size_t> current_ids;
      vector<size_t> old_ids;
      front_buffer.ids(current_ids);
      back_buffer.ids(old_ids);

      for (vector<size_t>::iterator it = current_ids.begin();
           it != current_ids.end(); ++it) {
        vector<size_t>::iterator p = find(old_ids.begin(), old_ids.end(), *it);
        if (p == old_ids.end()) {
          continue;
        }
        // 両方に同じ id があり、位置の変位が少なければ
        // no_move_indexes に登録する
        enum { MoveThreshold = 200 };
        long points_length = length<long>(front_buffer.point(*it),
                                          back_buffer.point(*p));
        if (points_length > MoveThreshold) {
          continue;
        }

        no_move_ids[type].push_back(*it);
      }

      // 移動変位の平均を求める
      calculateMoveOffset(no_move_ids[type], type);
    }

    // 推定の移動変位を計算する
    if (offset_total_size_ == 0) {
      ++offset_total_size_;
    }
    Point<long> move_offset = Point<long>(offset_total_.x / offset_total_size_,
                                          offset_total_.y / offset_total_size_);
    //fprintf(stderr, "[%d, %d], ", move_offset.x, move_offset.y);

    // 移動変位を加味した位置を地図に登録する
    for (size_t type = 0; type < Unknown; ++type) {
      margePoints(no_move_ids[type], type, move_offset);
    }

    // 推定位置への移動幅の反映
    const double offset_ratio = 0.6;
    position_.x += static_cast<long>(move_offset.x * offset_ratio);
    position_.y += static_cast<long>(move_offset.y * offset_ratio);

    // 加算値のクリア
    offset_total_ = Point<long>(0, 0);
    offset_total_size_ = 0;
  }
};


FeaturePointMap::FeaturePointMap(void) : pimpl(new pImpl)
{
}


FeaturePointMap::~FeaturePointMap(void)
{
}


void FeaturePointMap::setBufferPeriod(size_t msec)
{
  pimpl->period_msec_ = msec;
  pimpl->adjustBufferSize();
}


void FeaturePointMap::update(const std::vector<featurePoint_t>& feature_points,
                             const Position<long>& sensor_position,
                             long timestamp)
{
  if (feature_points.empty()) {
    return;
  }

  pimpl->update(feature_points, sensor_position, timestamp);
}


void FeaturePointMap::setPosition(const Position<long>& position)
{
  pimpl->position_ = position;
}


Position<long> FeaturePointMap::position(void) const
{
  return pimpl->position_;
}


Position<long> FeaturePointMap::velocity(void) const
{
  // !!! 速度を位置の変位の移動平均から生成する

  Position<long> dummy;
  return dummy;
}
