/*!
  \file
  \brief 環境マップ

  \author Satofumi KAMIMURA

  $Id: ObjectMap.cpp 1517 2009-11-15 15:24:14Z satofumi $
*/

#include "ObjectMap.h"
#include "ObjectMapBuffer.h"
#include <algorithm>
#include <map>

#include <cstdio>

using namespace qrk;
using namespace std;


namespace
{
  enum {
    DefaultGridWidth = 100,
  };

  typedef vector<size_t> Ids;
  typedef map<size_t, pole_t> Poles;
  typedef map<long, map<long, Ids> > PolesGrid;
  // !!! 直線用のデータ構造を定義
  typedef map<size_t, size_t> MergeTimes;

  //FILE* fd_ = NULL;
}


struct ObjectMap::pImpl
{
  long grid_width_;
  long timestamp_;
  PolesGrid poles_grid_;
  Poles poles_;
  // !!! 直線用のデータ構造を定義
  MergeTimes merge_times_;


  pImpl(size_t grid_width)
    : grid_width_(grid_width), timestamp_(InvalidTimestamp)
  {
    if (grid_width_ <= 0) {
      grid_width_ = 1;
    }

//     if (fd_ == NULL) {
//       fd_ = fopen("poles.txt", "w");
//     }
  }


  Ids& polesIds(const pole_t& pole)
  {
    Point<long> grid(pole.center.x / grid_width_,
                     grid.y = pole.center.y / grid_width_);
    return poles_grid_[grid.y][grid.x];
  }


  void addPole(const pole_t& pole, size_t id)
  {
    poles_[id] = pole;
    merge_times_[id] = 1;

    polesIds(pole).push_back(id);
  }


  void mergePole(size_t id, const pole_t& pole)
  {
    // !!! 浮動小数点以下を切り捨てる現在の実装が、良いか悪いかを評価する

    MergeTimes::iterator it = merge_times_.find(id);
    if (it == merge_times_.end()) {
      addPole(pole, id);
      //fprintf(stderr, "add, ");
      return;
    }

    int n = it->second;
    //fprintf(stderr, "<%d>, ", n);
    if (n > 20) {
      // 一定数以上の点データは加算しない
      // !!! 点の数をマクロにする
      //fprintf(stderr, "full, ");
      return;
    }

    //fprintf(stderr, "merge, ");
    pole_t original = poles_[id];
    Point<long> total(original.center.x * n, original.center.y * n);
    total.x += pole.center.x;
    total.y += pole.center.y;
    ++n;

    it->second = n;
    pole_t merged;
    merged.center.x = total.x / n;
    merged.center.y = total.y / n;
    poles_[id] = merged;

//     if (n == 5) {
//       fprintf(fd_, "%ld\t%ld\t# %d\n", merged.center.x, merged.center.y, id);
//       fflush(fd_);
//     }

    // merge すると位置が変わるので、位置を更新する
    Ids& poles = polesIds(original);
    poles.erase(std::remove(poles.begin(), poles.end(), id), poles.end());
    polesIds(merged).push_back(id);
  }


  void nearPoles(vector<size_t>& ids, const pole_t& pole, long radius)
  {
    long width = max(static_cast<long>(radius / grid_width_),
                     static_cast<long>(1));
    Point<long> grid(pole.center.x / grid_width_, pole.center.y / grid_width_);
    for (long y = grid.y - width; y <= grid.y + width; ++y) {
      for (long x = grid.x - width; x <= grid.x + width; ++x) {

        PolesGrid::const_iterator y_it = poles_grid_.find(y);
        if (y_it == poles_grid_.end()) {
          continue;
        }

        // !!! y_it を使うようにする
        map<long, Ids>::const_iterator x_it = poles_grid_[y].find(x);
        if (x_it == poles_grid_[y].end()) {
          continue;
        }

        const Ids& grid_poles = poles_grid_[y][x];
        ids.insert(ids.end(), grid_poles.begin(), grid_poles.end());
      }
    }
  }
};


ObjectMap::ObjectMap(void) : pimpl(new pImpl(DefaultGridWidth))
{
}


// ObjectMap::ObjectMap(size_t grid_width) : pimpl(new pImpl(grid_width))
// {
// }


ObjectMap::~ObjectMap(void)
{
}


void ObjectMap::setTimestamp(long timestamp)
{
  pimpl->timestamp_ = timestamp;
}


long ObjectMap::timestamp(void) const
{
  return pimpl->timestamp_;
}


void ObjectMap::addPole(const pole_t& pole, size_t id)
{
  pimpl->addPole(pole, id);
}


bool ObjectMap::poleExists(size_t id) const
{
  return (pimpl->poles_.find(id) == pimpl->poles_.end()) ? false : true;
}


void ObjectMap::mergePole(size_t id, const pole_t& pole)
{
  pimpl->mergePole(id, pole);
}


pole_t ObjectMap::pole(size_t id) const
{
  Poles::const_iterator it = pimpl->poles_.find(id);
  if (it != pimpl->poles_.end()) {
    return it->second;

  } else {
    pole_t dummy;
    dummy.center.x = -1;
    dummy.center.y = -1;
    dummy.radius = 1;
    return dummy;
  }
}


vector<size_t> ObjectMap::poles(void) const
{
  vector<size_t> ids;
  for (Poles::const_iterator it = pimpl->poles_.begin();
       it != pimpl->poles_.end(); ++it) {
    ids.push_back(it->first);
  }
  return ids;
}


void ObjectMap::nearPoles(std::vector<size_t>& ids,
                          const pole_t& pole, long radius) const
{
  pimpl->nearPoles(ids, pole, radius);
}


std::vector<size_t> ObjectMap::lines(void) const
{
  // !!!

  vector<size_t> dummy;
  return dummy;
}


void ObjectMap::nearLines(std::vector<size_t>& ids,
                          const line_t& line, long radius) const
{
  (void)ids;
  (void)line;
  (void)radius;

  // !!!
}
