#pragma once
#include <bitset>
#include <boost/unordered_map.hpp>
#include <boost/function.hpp>
#include <BWAPI.h>
#include <cmath>
#include <ctime>
#include <functional>
#include <list>
#include <set>
#include <utility>
#include "../../../BWAPI_Addon.h"
#include <boost/version.hpp>
#include "../../../PerceptualState/PerceptualState.h"
#include "../../../PerceptualState/InfluenceMap/Threat.h"
#include "../../../PerceptualState/InfluenceMap/Detection.h"
#include "../../../PerceptualState/Locations/Static/AutoPopulated/Chokepoint.h"

// TODO: pathfinding for ground units should use walk tiles (not build tiles!) + see BWTA::pathfinding

static const double NB_TILES_RETREAT = 6;

#if (BOOST_VERSION / 100000) < 1 || ((BOOST_VERSION / 100000) == 1 && (BOOST_VERSION / 100 % 1000) < 42)
  // Versions of Boost older than 1.42 seem to have a problem with
  // reinterpret_cast<>ed unordered_map::iterators... not sure why
  #pragma message("================================================================================")
  #pragma message("                  YOU SHOULD INSTALL BOOST 1.42 OR MORE RECENT")
  #pragma message("================================================================================")
#else
  #define PATHFINDING_USE_BOOST_UNORDERED_MAP
#endif

struct Path : public std::vector<BWAPI::TilePosition>
{
  Path(bool reachGoal_)
    : reachGoal(reachGoal_)
  {}
  bool reachGoal;
};
typedef boost::unordered_set<BWAPI::TilePosition, BWAPI::tile_position_hash> TileSet;
typedef std::vector<BWAPI::TilePosition> TileVector;
typedef boost::unordered_map<BWAPI::TilePosition, double> ScoreMap;
typedef unsigned int lookahead_t;
typedef boost::function<double(BWAPI::TilePosition)> ThreatEvaluator;
typedef boost::function<bool(BWAPI::TilePosition, BWAPI::TilePosition)> ObstacleEvaluator;

namespace pathfinding
{
  inline bool isValid(const BWAPI::TilePosition &pos)
  {
    return pos.x() >= 0 && pos.x() < Spar->getPerceptualState()->getMapWidth() && pos.y() >= 0 && pos.y() < Spar->getPerceptualState()->getMapHeight();
  }

  typedef unsigned int lookahead_t;
  typedef unsigned char predecessor_t;
  typedef std::vector<std::pair<::BWAPI::TilePosition,double>> Closed;

  class GeneralThreatEvaluator : public std::unary_function<BWAPI::TilePosition, double>
  {
  public:
    GeneralThreatEvaluator(BWAPI::Unit* unit)
      : m_unit(unit)
    {}
    double operator()(BWAPI::TilePosition tile) const
    {
      InfluenceMapRef<Threat>::map_ptr threatMap = Spar->getPerceptualState()->getEnemyThreat();
      const Threat & th = threatMap->getData(tile);
      double cost = 0;
      if(m_unit->getType().hasPermanentCloak())
      {
        InfluenceMapRef<Detection>::map_ptr detectionMap = Spar->getPerceptualState()->getDetection();
        cost += detectionMap->getData(tile).detectionLevel * 3.0;
        if(cost > 0.0)
          cost += (m_unit->getType().isFlyer() ? th.futureAir : th.futureGround);
      }
      else
      {
        cost += (m_unit->getType().isFlyer() ? th.futureAir : th.futureGround);
      }
      return cost;
    }
  protected:
    BWAPI::Unit* const m_unit;

    GeneralThreatEvaluator& operator=(const GeneralThreatEvaluator &);
  };

  class GeneralObstacleEvaluator : public std::binary_function<BWAPI::TilePosition, BWAPI::TilePosition, bool>
  {
  public:
    GeneralObstacleEvaluator(BWAPI::Unit* unit)
      : m_unit(unit)
    {}
    bool operator()(BWAPI::TilePosition pos1, BWAPI::TilePosition pos2) const
    {
      // TODO: doesn't really work, must be refined (e.g. check for units blocking the way as well, like minerals or neutral buildings)

      if (!m_unit->getType().isFlyer())
      {
        if (!isTileCompletelyWalkable(pos2))
          return true;
        const Tile* tileLocation = Spar->getPerceptualState()->getMap()->getTile(pos2);
        if (tileLocation->getNbOwnBuildings() > 0 ||
            tileLocation->getNbNeutralBuildings() > 0 ||
            tileLocation->getNbEnemyBuildings() > 0)
          return true;
        if (BWAPI::Broodwar->getGroundHeight(pos1) != BWAPI::Broodwar->getGroundHeight(pos2))
        {
          const BWTA::Chokepoint* chokepoint = BWTA::getNearestChokepoint(pos2);
          if (chokepoint == NULL || !Spar->getPerceptualState()->getMap()->getChokepoint(chokepoint)->isInLocation(pos2))
            return true;
        }
      }
      return false;
    }
  protected:
    static bool isTilePartiallyWalkable(BWAPI::TilePosition tile)
    {
      for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
        {
          if (BWAPI::Broodwar->isWalkable(tile.x() * 4 + i, tile.y() * 4 + j))
            return true;
        }
      return false;
    }
    static bool isTileCompletelyWalkable(BWAPI::TilePosition tile)
    {
      for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
        {
          if (!BWAPI::Broodwar->isWalkable(tile.x() * 4 + i, tile.y() * 4 + j))
            return false;
        }
      return true;
    }

    BWAPI::Unit* const m_unit;

    GeneralObstacleEvaluator& operator=(const GeneralObstacleEvaluator &);
  };

  class ConsiderThreatAsObstacle : public GeneralObstacleEvaluator
  {
  public:
    ConsiderThreatAsObstacle(BWAPI::Unit* unit)
      : GeneralObstacleEvaluator(unit)
    {}
    bool operator()(BWAPI::TilePosition pos1, BWAPI::TilePosition pos2) const
    {
      if (GeneralObstacleEvaluator::operator()(pos1, pos2))
      {
        return true;
      }
      else if (m_unit->getType().hasPermanentCloak() && Spar->getPerceptualState()->getDetection()->getData(pos2).detectionLevel <= 0)
      {
        return false;
      }
      else
      {
        // Unit may be detected
        const InfluenceMapRef<Threat>::map_ptr th = Spar->getPerceptualState()->getEnemyThreat();
        const Threat &t = th->getData(pos2);
        return m_unit->getType().isFlyer() ?  t.futureAir > 0 : t.futureGround > 0;
      }
    }
  protected:
    ConsiderThreatAsObstacle& operator=(const ConsiderThreatAsObstacle &);
  };

  struct Path
  {
    Path()
      : path(false), f(0)
    {}
    Path(bool reachGoal_, double f_)
      : path(reachGoal_), f(f_)
    {}
    ::Path path;
    double f;
  };

  //----------------------------------------------------------------------------

  static const double SQRT_OF_TWO = ::std::sqrt(2.0);

  static const predecessor_t SAME_NODE = 0;

  static const int DELTA_X[] = {0, -1,  0,  1, -1,  1, -1,  0,  1};
  static const int DELTA_Y[] = {0, -1, -1, -1,  0,  0,  1,  1,  1};

  //----------------------------------------------------------------------------

  template<class ObstacleEvaluator>
  ::std::bitset<8> getNeighbours(::BWAPI::TilePosition pos, ::BWAPI::TilePosition neighbours[8], const ObstacleEvaluator& isObstacle)
  {
    ::std::bitset<8> active;

    for (int i=0; i<8; ++i)
    {
      // ignore index 0, which corresponds to SAME_NODE
      neighbours[i].x() = pos.x() + DELTA_X[i+1];
      neighbours[i].y() = pos.y() + DELTA_Y[i+1];

      if (pathfinding::isValid(neighbours[i]) && !isObstacle(pos, neighbours[i]))
      {
        active.set(i);
      }
    }

    return active;
  }

  inline ::BWAPI::TilePosition getPredecessor(::BWAPI::TilePosition pos, predecessor_t pred)
  {
    return ::BWAPI::TilePosition(pos.x() - DELTA_X[pred], pos.y() - DELTA_Y[pred]);
  }

  //----------------------------------------------------------------------------

  struct NodeBase
  {
    double g, h;
    predecessor_t pred;
  };

#ifdef PATHFINDING_USE_BOOST_UNORDERED_MAP
  typedef ::boost::unordered_map<::BWAPI::TilePosition, NodeBase, ::BWAPI::tile_position_hash> OpenMapBase;
#else
  typedef stdext::hash_map<::BWAPI::TilePosition, NodeBase, ::BWAPI::tile_position_hash_ordering> OpenMapBase;
#endif

  typedef ::std::set<OpenMapBase::iterator> OpenBase;

  struct Node : NodeBase
  {
    OpenBase::iterator iter;
  };

#ifdef PATHFINDING_USE_BOOST_UNORDERED_MAP
  typedef boost::unordered_map<::BWAPI::TilePosition, Node, ::BWAPI::tile_position_hash> OpenMap;
#else
  typedef stdext::hash_map<::BWAPI::TilePosition, Node, ::BWAPI::tile_position_hash_ordering> OpenMap;
#endif

  struct FCostComparator
  {
    bool operator()(const OpenMap::iterator& i1, const OpenMap::iterator& i2) const
    {
      const double f1 = i1->second.g + i1->second.h;
      const double f2 = i2->second.g + i2->second.h;

      // min (g+h), min tile
      return (f1 < f2 || (f1 == f2 && i1->first < i2->first));
    }
  };

  struct AlternativeComparator
  {
    AlternativeComparator(BWAPI::TilePosition goal)
      : m_goal(goal)
    {}
    bool operator()(const OpenMap::iterator& i1, const OpenMap::iterator& i2) const
    {
      const double d1 = m_goal.getDistance(i1->first);
      const double d2 = m_goal.getDistance(i2->first);

      // min distance, min g, min tile
      if (d1 < d2)
        return true;
      else if (d1 == d2)
        if (i1->second.g < i2->second.g)
          return true;
        else if (i1->second.g == i2->second.g)
          return i1->first < i2->first;
        else
          return false;
      else
        return false;
    }
  private:
    const BWAPI::TilePosition m_goal;
    AlternativeComparator& operator=(const AlternativeComparator&);
  };

  struct GCostComparator
  {
    bool operator()(const OpenMap::iterator& i1, const OpenMap::iterator& i2) const
    {
      // min g, min h, min tile
      if (i1->second.g < i2->second.g)
        return true;
      else if (i1->second.g == i2->second.g)
        if (i1->second.h < i2->second.h)
          return true;
        else if (i1->second.h == i2->second.h)
          return i1->first < i2->first;
        else
          return false;
      else
        return false;
    }
  };

  //----------------------------------------------------------------------------

  template <class Open>
  inline void addToOpen(const OpenMap::iterator& iter, Open& open)
  {
    ::std::pair<typename Open::iterator,bool> open_iter = open.insert(iter); // O(log n)
    //assert(open_iter.second);
    iter->second.iter = reinterpret_cast<OpenBase::iterator&>(open_iter.first);
  }

  inline OpenMap::iterator addToOpenMap(::BWAPI::TilePosition pos, const Node& node, OpenMap& allTiles)
  {
    return allTiles.insert(::std::make_pair(pos, node)).first; // O(1)
  }

  template <class Open>
  inline void removeFromOpen(const typename Open::iterator& iter, OpenMap::iterator& backref, Open& open)
  {
    assert(reinterpret_cast<typename Open::iterator&>(backref->second.iter) == iter);
    open.erase(iter); // O(log n)
    typename Open::iterator end = open.end();
    backref->second.iter = reinterpret_cast<OpenBase::iterator&>(end);
  }

  template <class Open>
  inline OpenMap::iterator getBestNode(Open& open)
  {
    typename Open::iterator iter = open.begin();
    OpenMap::iterator best = *iter;
    removeFromOpen(iter, best, open);
    return best;
  }
} // namespace pathfinding

//==============================================================================

/**
 * @brief Returns the position of the last point in a path that makes a straight line, i.e. the path curves after said point. 
 * A "point" is an object with two methods, x() and y(), that return the x and y coordinates of the point, respectively
 * @param begin The start of the path
 * @param end The end of the path
 */
template<class Iterator>
Iterator findEndOfStraightLine(Iterator begin, Iterator end)
{
  Iterator previous = begin;
  ++begin;
  if(begin == end)
    return end;
  double lastSlope = static_cast<double>(begin->y() - previous->y()) / static_cast<double>(begin->x() - previous->x());
  
  for(++previous, ++begin; begin != end; ++previous, ++begin)
  {
    double slope = static_cast<double>(begin->y() - previous->y()) / static_cast<double>(begin->x() - previous->x());
    if(std::fabs(slope - lastSlope) > 0.001)
      break;
    
    lastSlope = slope;
  }
  return previous;
}

template <class Iterator>
std::vector<BWAPI::TilePosition> smoothPath(Iterator begin, Iterator end)
{
  std::vector<BWAPI::TilePosition> path;
  for(Iterator it = begin; it != end;)
  {
    Iterator next = findEndOfStraightLine(it, end);
    if(next == end)
      break;

    path.push_back(*next);
    it = ++next;
  }
  return path;
}

inline pathfinding::Path reconstructPath(
  const pathfinding::OpenMap& allTiles, 
  BWAPI::TilePosition end, 
  const pathfinding::Node* node,
  bool reachGoal)
{
  pathfinding::Path path(reachGoal, node->g + node->h);

  path.path.push_back(end);
  while (node->pred != pathfinding::SAME_NODE)
  {
    pathfinding::OpenMap::const_iterator x = allTiles.find(pathfinding::getPredecessor(end, node->pred)); // O(1)
    end = x->first;
    node = &x->second;
    path.path.push_back(end);
  }
  std::reverse(path.path.begin(), path.path.end());
  return path;
}

template<class Heuristic, class ObstacleEvaluator, class ThreatEvaluator>
pathfinding::Path boundedAStar(
  const BWAPI::TilePosition init,
  const BWAPI::TilePosition goal,
  pathfinding::Closed& closed,
  pathfinding::lookahead_t lookahead,
  const Heuristic& h,
  const ObstacleEvaluator& isObstacle,
  const ThreatEvaluator& threatEvaluator,
  const double moveWeight)
{
  using pathfinding::OpenMap;
  using pathfinding::Node;
  typedef std::set<OpenMap::iterator, pathfinding::FCostComparator> Open;

  // TODO: try implementing allTiles as a fixed array of mapWidth() * mapHeight()

  OpenMap allTiles;
#ifdef PATHFINDING_USE_BOOST_UNORDERED_MAP
  static const size_t MIN_NB_BUCKETS = 10000;
  static const float MAX_LOAD_FACTOR = 30;
  const size_t maxNbBuckets = std::max(MIN_NB_BUCKETS, (size_t)((float)(Spar->getPerceptualState()->getMapWidth() * Spar->getPerceptualState()->getMapHeight() * 16) / MAX_LOAD_FACTOR));
  allTiles.rehash(maxNbBuckets);
  allTiles.max_load_factor(boost::numeric::bounds<float>::highest()); // the hash-map must *never* rehash since this invalidates iterators
#endif

  Open open;
  const pathfinding::AlternativeComparator comparator(goal);

  // Insert initial state in open
  {
    Node node;
    node.g = 0;
    node.h = h(init, goal);
    node.pred = pathfinding::SAME_NODE;

    OpenMap::iterator iter = pathfinding::addToOpenMap(init, node, allTiles);
    addToOpen(iter, open);
  }

  OpenMap::iterator best = allTiles.end();
  while(!open.empty())
  {
    OpenMap::iterator x = pathfinding::getBestNode(open);
    const BWAPI::TilePosition xTile = x->first;
    Node& xNode = x->second;
    
    closed.push_back(std::make_pair(xTile, xNode.g));
    --lookahead;

    if (lookahead == 0 || xTile == goal)
    {
      return reconstructPath(allTiles, xTile, &xNode, xTile == goal);
    }
    else if (xTile != init && (best == allTiles.end() || comparator(x, best)))
    {
      best = x;
    }

    BWAPI::TilePosition neighbours[8];
    std::bitset<8> active = pathfinding::getNeighbours(xTile, neighbours, isObstacle);
    for (int i=0; i<8; ++i)
    {
      if (!active.test(i)) continue;

      const BWAPI::TilePosition y = neighbours[i];
      //if (closed.find(y) != closed.end()) continue; // O(log n)

      const double cost = (xTile.x() == y.x() || xTile.y() == y.y() ? 1.0 : pathfinding::SQRT_OF_TWO);
      const double g = xNode.g + moveWeight * cost + threatEvaluator(y);

      OpenMap::iterator iter = allTiles.find(y); // O(1)
      if (iter == allTiles.end())
      {
        Node node;
        node.g = g;
        node.h = h(y, goal);
        node.pred = static_cast<pathfinding::predecessor_t>(i + 1);

        OpenMap::iterator openmap_iter = pathfinding::addToOpenMap(y, node, allTiles);
        pathfinding::addToOpen(openmap_iter, open);
      }
      else
      {
        Node& node = iter->second;
        Open::iterator open_iter = reinterpret_cast<Open::iterator&>(node.iter);
        
        if (open_iter == open.end())
        {
          // node is in closed
          //assert(closed.find(iter->first) != closed.end());
        }
        else if (g < node.g)
        {
          // node is in open; update it with better path
          pathfinding::removeFromOpen(open_iter, iter, open);

          node.g = g;
          node.pred = static_cast<pathfinding::predecessor_t>(i + 1);

          pathfinding::addToOpen(iter, open);
        }
      }
    }
  }

  // No path from `init' to `goal' found

  if (best == allTiles.end())
    return pathfinding::Path();
  else
    return reconstructPath(allTiles, best->first, &best->second, best->first == goal);
}

///**
// * @brief Finds the safest air path between two tiles 
// * @param unit The unit to calculate the path for.
// * @param end The destination tile.
// * @param score A map that shall contains the "g" value of the expanded tiles.
// * @param closed A set containing the expanded sets.
// * @param lookahead The maximum amount of tiles to expand.
// * @param evaluator A function object that evaluates the "threat" of a certain tile.
// */
//Path boundedAStar(
//  const TilePosition &start,
//  const TilePosition &goal,
//  TileSet &closed,
//  lookahead_t lookahead,
//  const clock_t cutoff,
//  ScoreMap &score,
//  TileComparer cmp,
//  ThreatEvaluator evaluator,
//  ObstacleEvaluator isObstacle)
//{
//  typedef boost::unordered_map<TilePosition, TilePosition> PredecessorMap;
//
//  TileVector open;
//
//  open.push_back(start);
//  score.insert(std::make_pair(start, 0.0));
//  PredecessorMap previous;
//
//  const clock_t startTime = clock();
//  const clock_t expectedEnd = startTime + cutoff;
//  while (!open.empty())
//  {
//    std::pop_heap(open.begin(), open.end(), cmp);
//    TilePosition x = open.back();
//    open.pop_back();
//    clock_t currentTime = clock();
//    clock_t timeleft = expectedEnd - currentTime;
//
//    if(timeleft <= 0)
//    {
//      SPAR_LOG(LogTypes.GENERAL_WARNING, "PATH PLANNING DID NOT FINISH (%u)", lookahead);
//    }
//
//    if(lookahead == 0 || x == goal || timeleft <= 0 ) 
//    {
//      assert(lookahead >= 0);
//      Path path(x == goal);
//      PredecessorMap::const_iterator pit = previous.find(x);
//      while (pit != previous.end())
//      {
//        path.push_back(pit->first);
//        pit = previous.find(pit->second);
//      }
//      path.push_back(start);
//
//      std::reverse(path.begin(), path.end());
//      return path;
//    }
//
//    --lookahead;
//
//    closed.insert(x);
//
//    TilePosition neighbors[8];
//    const int realSize = getNeighbors(x, isObstacle, neighbors); 
//    for(int i = 0; i < realSize; ++i)
//    {
//      const TilePosition &y = neighbors[i];
//      if(closed.find(y) != closed.end())
//        continue;
//
//      const double movCost = (x.x() == y.x() || x.y() == y.y() ? 1.0 : SQRT_OF_TWO);
//      double tempScore = score[x] + evaluator(y) + movCost;
//
//      ScoreMap::iterator it = score.find(y);
//      if (it == score.end() || tempScore < it->second)
//      {
//        score[y] = tempScore;
//        previous[y] = x;
//
//        open.push_back(y);
//        std::make_heap(open.begin(), open.end(), cmp);
//      }
//    }
//  }
//
//  return Path(false);
//}

inline double calculateThreatAround(
  BWAPI::TilePosition tile,
  const ThreatEvaluator& threatEvaluator)
{
  assert(tile.isValid());
  double sum = 0;
  for (int x = -1; x <= 1; ++x)
  {
    for (int y = -1; y <= 1; ++y)
    {
      const BWAPI::TilePosition pos = tile + BWAPI::TilePosition(x,y);
      if (pos.isValid() && tile != pos)
      {
        sum += threatEvaluator(pos);
      }
    }
  }
  return sum;
}

template<class ObstacleEvaluator, class ThreatEvaluator>
pathfinding::Path calculateRetreatPath(
  BWAPI::Unit* unit,
  const ObstacleEvaluator& isObstacle,
  const ThreatEvaluator& threatEvaluator,
  const size_t maxNodesToExplore = 500)
{
  // Find the (optimal) lowest-cost path that leads to the lowest-cost tile (hopefully a tile with 0 threat).

  // Note that Node::h is used as the distance from the initial position, instead of the heuristic value
  // Should be fast to calculate...

  // TODO: could bias the paths towards the center of the map, so that this path is preferred to staying immobile
  //       (and next call will again steer the unit towards the center if nothing else is possible)

  using pathfinding::OpenMap;
  using pathfinding::Node;
  typedef std::set<OpenMap::iterator, pathfinding::GCostComparator> Open;

  OpenMap allTiles;
#ifdef PATHFINDING_USE_BOOST_UNORDERED_MAP
  static const size_t MIN_NB_BUCKETS = 10000;
  static const float MAX_LOAD_FACTOR = 30;
  const size_t maxNbBuckets = std::max(MIN_NB_BUCKETS, (size_t)((float)(Spar->getPerceptualState()->getMapWidth() * Spar->getPerceptualState()->getMapHeight() * 16) / MAX_LOAD_FACTOR));
  allTiles.rehash(maxNbBuckets);
  allTiles.max_load_factor(boost::numeric::bounds<float>::highest()); // the hash-map must *never* rehash since this invalidates iterators
#endif

  const BWAPI::TilePosition init = unit->getTilePosition();
  Open open;
  // Insert initial state in open
  {
    Node node;
    node.g = 0;
    node.h = 0;
    node.pred = pathfinding::SAME_NODE;

    OpenMap::iterator iter = pathfinding::addToOpenMap(init, node, allTiles);
    addToOpen(iter, open);
  }

  size_t nbNodesExplored = 0;
  OpenMap::iterator bestNode = allTiles.begin();
  double bestThreat = boost::numeric::bounds<double>::highest();
  double bestDistance = 0;
  while(!open.empty() && ++nbNodesExplored <= maxNodesToExplore)
  {
    OpenMap::iterator x = pathfinding::getBestNode(open);
    const BWAPI::TilePosition xTile = x->first;
    Node& xNode = x->second;

    BWAPI::TilePosition neighbours[8];
    std::bitset<8> active = pathfinding::getNeighbours(xTile, neighbours, isObstacle);
    for (int i=0; i<8; ++i)
    {
      if (!active.test(i)) continue;

      const BWAPI::TilePosition y = neighbours[i];
      const double cost = (xTile.x() == y.x() || xTile.y() == y.y() ? 1.0 : pathfinding::SQRT_OF_TWO);
      const double threat = threatEvaluator(y);
      const double g = xNode.g + cost * threat;
      const double h = calculateThreatAround(xTile, threatEvaluator);

      OpenMap::iterator iter = allTiles.find(y); // O(1)
      if (iter == allTiles.end())
      {
        Node node;
        node.g = g;
        node.h = h;
        node.pred = static_cast<pathfinding::predecessor_t>(i + 1);

        iter = pathfinding::addToOpenMap(y, node, allTiles);
        pathfinding::addToOpen(iter, open);
      }
      else
      {
        Node& node = iter->second;
        Open::iterator open_iter = reinterpret_cast<Open::iterator&>(node.iter);
        
        if (open_iter == open.end())
        {
          // node is in closed
        }
        else if (g < node.g || (g == node.g && h < node.h))
        {
          // node is in open; update it with better path
          pathfinding::removeFromOpen(open_iter, iter, open);

          node.g = g;
          node.h = h;
          node.pred = static_cast<pathfinding::predecessor_t>(i + 1);

          pathfinding::addToOpen(iter, open);
        }
      }
      // min threat, min g, min h, min |NB_TILES_RETREAT - n|
      const double distance = iter->first.getDistance(init);
      if (threat < bestThreat || 
        (threat == bestThreat && (iter->second.g < bestNode->second.g ||
          (iter->second.g == bestNode->second.g && (iter->second.h < bestNode->second.h ||
            (iter->second.h == bestNode->second.h && std::abs(distance - NB_TILES_RETREAT) < std::abs(bestDistance - NB_TILES_RETREAT)
          ))))))
      {
        bestNode = iter;
        bestThreat = threat;
        bestDistance = distance;
        // g is necessarily minimal
        if (bestThreat == 0 && bestNode->second.h == 0 && std::abs(bestDistance - NB_TILES_RETREAT) < 0.5) // Can't be much better than that
          return reconstructPath(allTiles, bestNode->first, &bestNode->second, false);
      }
    }
  }

  // No path from `init' to `goal' found

  if (bestNode->first == init)
    return pathfinding::Path();
  else
    return reconstructPath(allTiles, bestNode->first, &bestNode->second, false);
}

inline pathfinding::Path calculateRetreatPath(
  BWAPI::Unit* unit)
{
  return calculateRetreatPath<pathfinding::GeneralObstacleEvaluator, pathfinding::GeneralThreatEvaluator>(
    unit,
    pathfinding::GeneralObstacleEvaluator(unit),
    pathfinding::GeneralThreatEvaluator(unit));
}