/*!
  \file
  \brief 経路探索

  boost 付属サンプルの astar-cities.cpp を参考にして実装した。

  \author Satofumi KAMIMURA

  $Id: SearchPath.cpp 1354 2009-10-02 10:59:07Z satofumi $
*/

#include "SearchPath.h"
#include "MathUtils.h"
#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <list>
#include <iostream>

using namespace qrk;
using namespace boost;
using namespace std;


namespace
{
  typedef vector<char> Line;
  typedef vector<Line> Grid;


  struct location
  {
    long x;
    long y;


    location(long x_, long y_) : x(x_), y(y_)
    {
    }
  };


  template <class Graph, class CostType, class LocMap>
  class distance_heuristic : public astar_heuristic<Graph, CostType>
  {
  public:
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    distance_heuristic(LocMap l, Vertex goal)
      : m_location(l), m_goal(goal)
    {
    }

    CostType operator()(Vertex u)
    {
      CostType dx = m_location[m_goal].x - m_location[u].x;
      CostType dy = m_location[m_goal].y - m_location[u].y;
      return sqrt((dx * dx) + (dy * dy));
    }
  private:
    LocMap m_location;
    Vertex m_goal;
  };


  struct found_goal {};


  template <class Vertex>
  class astar_goal_visitor : public boost::default_astar_visitor
  {
  public:
    astar_goal_visitor(Vertex goal) : m_goal(goal)
    {
    }

    template <class Graph>
    void examine_vertex(Vertex u, Graph& g)
    {
      static_cast<void>(g);
      if(u == m_goal)
        throw found_goal();
    }

  private:
    Vertex m_goal;
  };
}


struct SearchPath::pImpl
{
  Grid grid_;
  size_t width_;
  size_t height_;
  Point<long> start_;
  Point<long> goal_;

  Point<long> origin_;
  long magnify_;


  pImpl(size_t width, size_t height)
    : width_(width), height_(height), magnify_(1)
  {
    clear();
  }


  void clear(void)
  {
    grid_.clear();
    for (size_t i = 0; i < height_; ++i) {
      Line line;
      line.resize(width_, ' ');
      grid_.push_back(line);
    }
  }


  bool path(std::vector<Point<long> >& points) const
  {
    typedef float cost;
    typedef adjacency_list<listS, vecS, undirectedS, no_property,
      property<edge_weight_t, cost> > graph_t;
    typedef property_map<graph_t, edge_weight_t>::type WeightMap;
    typedef graph_t::vertex_descriptor vertex;
    typedef graph_t::edge_descriptor edge_descriptor;
    typedef pair<int, int> edge;

    // グラフの生成
    size_t num_nodes = 0;
    vector<edge> edges;
    vector<cost> weights;
    vector<location> locations;
    const float sqrt_2 = sqrt(2.0);
    for (size_t y = 0; y < height_; ++y) {
      for (size_t x = 0; x < width_; ++x) {
        int current_index = (width_ * y) + x;
        locations.push_back(location(x, y));
        ++num_nodes;
        if (grid_[y][x] == '#') {
          // ノードのみ作成し、エッジを作らない
          continue;
        }

        // ノードが有効な箇所にエッジを作成する
        bool right = false;
        bool right_down = false;
        if (x < (width_ - 1)) {
          right = (grid_[y][x + 1] == ' ') ? true : false;

          if (y < (height_ - 1)) {
            right_down = (grid_[y + 1][x + 1] == ' ') ? true : false;
          }
        }

        bool left = false;
        bool left_down = false;
        if (x > 0) {
          left = (grid_[y][x - 1] == ' ') ? true : false;

          if (y < (height_ - 1)) {
            left_down = (grid_[y + 1][x - 1] == ' ') ? true : false;
          }
        }

        bool down = false;
        if (y < (height_ - 1)) {
          down = (grid_[y + 1][x] == ' ') ? true : false;
        }

        if (right) {
          edges.push_back(edge(current_index, current_index + 1));
          weights.push_back(1.0);
        }

        if (down) {
          edges.push_back(edge(current_index, current_index + width_));
          weights.push_back(1.0);
        }

        if (left_down && (left || down)) {
          edges.push_back(edge(current_index, current_index + width_ - 1));
          weights.push_back(sqrt_2);
        }

        if (right_down && (right || down)) {
          edges.push_back(edge(current_index, current_index + width_ + 1));
          weights.push_back(sqrt_2);
        }
      }
    }

    graph_t g(num_nodes);
    WeightMap weightmap = get(edge_weight, g);
    size_t num_edges = edges.size();
    for(std::size_t i = 0; i < num_edges; ++i) {
      edge_descriptor e;
      bool inserted;
      tie(e, inserted) = add_edge(edges[i].first, edges[i].second, g);
      weightmap[e] = weights[i];
    }

    vector<graph_t::vertex_descriptor> p(num_vertices(g));

    vertex start = (start_.y * width_) + start_.x;
    vertex goal = (goal_.y * width_) + goal_.x;

    vector<cost> d(num_vertices(g));

    try {
      astar_search
        (g, start,
         distance_heuristic<graph_t, cost, location*>(&locations[0], goal),
         predecessor_map(&p[0]).distance_map(&d[0]).
         visitor(astar_goal_visitor<vertex>(goal)));

    } catch(found_goal fg) {
      // 経路が見つかっていなければ、戻る
      list<vertex> shortest_path;
      for (vertex v = goal; ; v = p[v]) {
        shortest_path.push_front(v);
        if (p[v] == v) {
          break;
        }
      }

      // 経路の登録
      list<vertex>::iterator spi = shortest_path.begin();
      for (++spi; spi != shortest_path.end(); ++spi) {
        long y = (*spi / width_) - origin_.y;
        long x = (*spi % width_) - origin_.x;

        points.push_back(Point<long>(x * magnify_, y * magnify_));
      }
      return true;
    }

    return false;
  }
};


SearchPath::SearchPath(size_t width, size_t height)
  : pimpl(new pImpl(width, height))
{
}


SearchPath::~SearchPath(void)
{
}


void SearchPath::clear(void)
{
  pimpl->clear();
}


void SearchPath::setObstacle(long x, long y)
{
  Point<long> obstacle((x / pimpl->magnify_) + pimpl->origin_.x,
                       (y / pimpl->magnify_) + pimpl->origin_.y);
  if ((obstacle.x < 0) || (obstacle.x >= static_cast<long>(pimpl->width_)) ||
      (obstacle.y < 0) || (obstacle.y >= static_cast<long>(pimpl->height_))) {
    return;
  }
  pimpl->grid_[obstacle.y][obstacle.x] = '#';
}


void SearchPath::unsetObstacle(long x, long y)
{
  Point<long> obstacle((x / pimpl->magnify_) + pimpl->origin_.x,
                       (y / pimpl->magnify_) + pimpl->origin_.y);
  if ((obstacle.x < 0) || (obstacle.x >= static_cast<long>(pimpl->width_)) ||
      (obstacle.y < 0) || (obstacle.y >= static_cast<long>(pimpl->height_))) {
    return;
  }
  pimpl->grid_[obstacle.y][obstacle.x] = ' ';
}


void SearchPath::setStart(long x, long y)
{
  // !!! 範囲外のときに、例外を投げる

  Point<long> start((x / pimpl->magnify_) + pimpl->origin_.x,
                    (y / pimpl->magnify_) + pimpl->origin_.y);
  unsetObstacle(x, y);
  pimpl->start_ = start;
}


void SearchPath::setGoal(long x, long y)
{
  // !!! 範囲外のときに、例外を投げる

  Point<long> goal((x / pimpl->magnify_) + pimpl->origin_.x,
                    (y / pimpl->magnify_) + pimpl->origin_.y);
  unsetObstacle(x, y);
  pimpl->goal_ = goal;
}


void SearchPath::setOrigin(size_t x, size_t y)
{
  pimpl->origin_ = Point<long>(x, y);
}


void SearchPath::setMagnify(int magnify)
{
  pimpl->magnify_ = magnify;
}


bool SearchPath::path(std::vector<Point<long> >& points) const
{
  return pimpl->path(points);
}
