/*!
  \file
  \brief 環境マップの補助関数

  \author Satofumi KAMIMURA

  $Id$
*/

#include "ObjectMapUtils.h"
#include "ObjectMapBuffer.h"
#include "ObjectMap.h"
#include "AngleUtils.h"
#include "PointUtils.h"

using namespace qrk;
using namespace std;


namespace
{
  enum {
    findBufferNum = 4,
    NearLength = 200,           // [mm]
  };


  void commonIds(vector<size_t>& exists_ids,
                 const ObjectMap* global_map, const ObjectMap* current_map)
  {
    // 共通に存在する ID を抽出
    vector<size_t> current_ids = current_map->poles();
    for (vector<size_t>::iterator it = current_ids.begin();
         it != current_ids.end(); ++it) {
      size_t id = *it;
      if (global_map->poleExists(id)) {
        exists_ids.push_back(id);
      }
    }
  }

  Angle adjustAngleDiff(const Angle& angle)
  {
    int degree = angle.to_deg();
    if (degree < -180) {
      return angle + deg(360);
    } else if (degree > +180) {
      return angle - deg(360);
    } else {
      return angle;
    }
  }


  bool nearPoints(const Point<long>& a, const Point<long>& b,
                  int threshold_length)
  {
    if (length<long>(a, b) <= threshold_length) {
      return true;
    } else {
      return false;
    }
  }
}


Angle qrk::angleFromPoles(const ObjectMap* global_map,
			  const ObjectMap* current_map)
{
  vector<size_t> exists_ids;
  commonIds(exists_ids, global_map, current_map);

  int n = exists_ids.size();
  if (n < 2) {
    // 直線が作れない場合は、推定を行わない
    return deg(0);
  }

  // 柱を端点とした直線を作り、その直線の角度のずれから角度変位を推定する
  size_t first_id = exists_ids.front();
  pole_t current_first = current_map->pole(first_id);
  pole_t global_first = global_map->pole(first_id);
  Angle total_angle;

  //fprintf(stderr, "<%d>, ", first_id);
  pole_t pole = current_map->pole(first_id);
  //fprintf(stderr, "[%ld,%ld], ", pole.center.x, pole.center.y);

  for (vector<size_t>::iterator it = exists_ids.begin() + 1;
       it != exists_ids.end(); ++it) {
    size_t id = *it;
    //fprintf(stderr, "<%d>, ", id);
    pole = current_map->pole(id);
    //fprintf(stderr, "[%ld,%ld], ", pole.center.x, pole.center.y);
    Angle current_angle =
      lineDirection<long>(current_first.center, current_map->pole(id).center);
    Angle global_angle =
      lineDirection<long>(global_first.center, global_map->pole(id).center);

    total_angle += adjustAngleDiff(global_angle - current_angle);
    //fprintf(stderr, "[%d,%d],", global_angle.to_deg(), current_angle.to_deg());
    //fprintf(stderr, "{%ld}, ", length<long>(current_first.center, current_map->pole(id).center));
  }

  //fprintf(stderr, "%d [deg], ", total_angle.to_deg() / (n - 1));
  return rad(total_angle.to_rad() / (n - 1));
}


Point<long> qrk::pointFromPoles(const ObjectMap* global_map,
				const ObjectMap* current_map)
{
  vector<size_t> exists_ids;
  commonIds(exists_ids, global_map, current_map);

  //fprintf(stderr, "Matched: %d, ", exists_ids.size());

  if (exists_ids.empty()) {
    return Point<long>(0, 0);
  }

  Point<long> total_point;
  for (vector<size_t>::iterator it = exists_ids.begin();
       it != exists_ids.end(); ++it) {
    size_t id = *it;
    Point<long> current_point = current_map->pole(id).center;
    //fprintf(stderr, "<%ld,%ld>, ", current_point.x, current_point.y);
    Point<long> global_point = global_map->pole(id).center;

    //fprintf(stderr, "{%ld,%ld}-{%ld,%ld} ", global_point.x, global_point.y, current_point.x, current_point.y);

    total_point += global_point - current_point;
  }
  int n = exists_ids.size();
  total_point.x /= n;
  total_point.y /= n;

  //fprintf(stderr, "<<%ld, %ld>>, ", total_point.x, total_point.y);

  return total_point;
}


void qrk::registerPoles(ObjectMapBuffer& map_buffer,
                        const ObjectMap& global_map,
                        const std::vector<pole_t>& poles, long timestamp)
{
  size_t index = map_buffer.index(timestamp);
  size_t map_size = min(map_buffer.size() - index,
			static_cast<size_t>(findBufferNum));

  //fprintf(stderr, "new ");
  ObjectMap* map = map_buffer.at(index);
  for (vector<pole_t>::const_iterator it = poles.begin();
       it != poles.end(); ++it) {
    bool assigned = false;
    for (size_t i = index + 1; i < map_size; ++i) {

      vector<size_t> ids;
      ObjectMap* compare_map = map_buffer.at(i);
      compare_map->nearPoles(ids, *it, NearLength);

      // 以前のデータの近い位置に pole が１つだけあれば、同じ ID を割り当てる
      if (ids.size() == 1) {
        int matched_id = ids.front();
        if (nearPoints(it->center,
                       compare_map->pole(matched_id).center, NearLength)) {
          map->addPole(*it, matched_id);
          assigned = true;
          //fprintf(stderr, "near: %d, ", ids.front());
          //fprintf(stderr, "(%ld, %ld), ", it->center.x, it->center.y);
          break;
        }
      }
    }
    if (! assigned) {
      // 見付からなければ、global_map から ID を探す
      vector<size_t> ids;
      global_map.nearPoles(ids, *it, NearLength);

      size_t last_id;
      if ((ids.size() == 1) &&
          nearPoints(it->center,
                     global_map.pole(ids.front()).center, NearLength)) {
        last_id = ids.front();
      } else {
        last_id = map_buffer.lastId();
      }

      // global_map の ID か新規 ID を登録する
      map->addPole(*it, last_id);
      //fprintf(stderr, "new id, ", last_id);
      //fprintf(stderr, "(%ld,%ld)", it->center.x, it->center.y);
    }
  }
  //fprintf(stderr, ", ");
}


void qrk::registerLines(ObjectMapBuffer& map_buffer,
                        const std::vector<line_t>& lines, long timestamp)
{
  (void)map_buffer;
  (void)lines;
  (void)timestamp;

  // !!!
}


void qrk::mergeStaticObject(ObjectMap& global_map,
                            ObjectMapBuffer& map_buffer,
                            size_t buffer_msec)
{
  if (map_buffer.empty()) {
    return;
  }

  long last_timestamp = map_buffer.front()->timestamp() - buffer_msec;
  //fprintf(stderr, "%ld, ", last_timestamp);

  // 保持時間を過ぎたバッファを登録して削除する
  for (ObjectMap* last_map = map_buffer.back();
       last_map->timestamp() < last_timestamp; last_map = map_buffer.back()) {

    // poles のマージを行う
    vector<size_t> pole_ids = last_map->poles();
    for (vector<size_t>::const_iterator it = pole_ids.begin();
	 it != pole_ids.end(); ++it) {
      size_t id = *it;

      // !!! 移動物体として判定された pole は登録しないようにする

      if (! global_map.poleExists(id)) {
	// !!! 近くに柱が複数あれば登録しない、ようにする
	// !!! 近くに柱があれば、その柱と同じ位置に登録する、ようにする
	global_map.addPole(last_map->pole(id), id);

	//pole_t pole = global_map.pole(*it);
	//fprintf(stderr, "[%ld, %ld], ", pole.center.x, pole.center.y);

      } else {
	global_map.mergePole(id, last_map->pole(id));

	//pole_t current_pole = last_map->pole(*it);
	//fprintf(stderr, "[%ld, %ld], ", current_pole.center.x, current_pole.center.y);
	//pole_t pole = global_map.pole(*it);
	//fprintf(stderr, "[%ld, %ld], ", pole.center.x, pole.center.y);
      }
    }

    // lines のマージを行う
    // !!!

    map_buffer.pop_back();
    //fprintf(stderr, "pop_back(), ");
  }
}


void qrk::updateObjectPosition(ObjectMap* current_map,
			       const Position<long>& position,
			       const Position<long>& previous_position)
{
  (void)current_map;
  (void)position;
  (void)previous_position;

  // !!!
}
