#include "stdafx.h"
#include "World/CurrentWorld.h"
#include "Processes/MonitoringProcess/UnitCreationMonitoringProcess.h"
#include "Processes/MonitoringProcess/UnitTrainingMonitoringProcess.h"
#include "Processes/MonitoringProcess/TechMonitoringProcess.h"
#include "Processes/MonitoringProcess/UpgradeMonitoringProcess.h"
#include "Locations/Static/AutoPopulated/BaseLocation.h"
#include "UnitRecord/UnitRecordManager.h"
#include "PerceptualState.h"
#include "../Utils/BresenhamCircle.h"
#include <algorithm>
#include <cassert>
#include <cmath>
#include <functional>
#include <limits>
#include <boost/numeric/conversion/bounds.hpp>

double ray_segment_intersection(const Vector2f& point, const double angle, const Vector2f& p1, const Vector2f& p2, Vector2f& intersection);

PerceptualState::PerceptualState(LocationFactory& locationFactory)
  : Process(Spar)
  , m_locationFactory(locationFactory)
  , m_unitCompletedEvent(*this)
  , m_unitRecordCreateEvent(*this)
  , m_unitRecordDestroyEvent(*this)
  , m_unitRecordMorphEvent(*this)
  , m_unitRecordRenegadeEvent(*this)
  , m_unitProcessTerminateEventHandler(*this)
  , m_unitRecordDestroyEventHandler(*this)
  , m_unitRecordMorphEventHandler(*this)
  , m_unitRecordRenegadeEventHandler(*this)
  , m_unitDiscoverEventHandler(*this)
  , m_unitEvadeEventHandler(*this)
  , m_unitMorphEventHandler(*this) 
  , m_unitRenegadeEventHandler(*this) 
  , m_unitDestroyEventHandler(*this) 
  , m_enemyThreatInfluenceMap(BWAPI::Broodwar->mapWidth(), BWAPI::Broodwar->mapHeight(), BWAPI::Broodwar->enemy())
  , m_selfThreatInfluenceMap(BWAPI::Broodwar->mapWidth(), BWAPI::Broodwar->mapHeight(), BWAPI::Broodwar->self())
  , m_visibilityInfluenceMap(BWAPI::Broodwar->mapWidth(), BWAPI::Broodwar->mapHeight(), BWAPI::Broodwar->self())
  //, m_structureInfluenceMap(BWAPI::Broodwar->mapWidth(), BWAPI::Broodwar->mapHeight(), BWAPI::Broodwar->enemy())
  , m_detectionInfluenceMap(BWAPI::Broodwar->mapWidth(), BWAPI::Broodwar->mapHeight(), BWAPI::Broodwar->enemy())
  , m_vulnerabilityInfluenceMap(BWAPI::Broodwar->mapWidth(), BWAPI::Broodwar->mapHeight(), BWAPI::Broodwar->enemy())
  , m_updateEnemyThreatInfluenceMap(this, m_enemyThreatInfluenceMap, 1, 1)
  , m_updateSelfThreatInfluenceMap(this, m_selfThreatInfluenceMap, 1, 2)
  , m_updateVisibilityInfluenceMap(this, m_visibilityInfluenceMap, 1, 3)
  //, m_updateStructuralImportanceInfluenceMap(this, m_structureInfluenceMap, 20, 4)
  , m_updateDetectionInfluenceMap(this, m_detectionInfluenceMap, 1, 5)
  , m_updateVulnerabilityInfluenceMap(this, m_vulnerabilityInfluenceMap, 4, 6)
  , m_updateUnits(this, &m_map, m_pathPrediction)
  , m_mapWidth(BWAPI::Broodwar->mapWidth())
  , m_mapHeight(BWAPI::Broodwar->mapHeight())
{
  m_map._markAsStatic();

  UnitRecordManager::initialize();

  m_map.initialize(m_locationFactory);

  m_unitDiscoverID = BWAPI::getUnitDiscoverEvent().subscribe(&m_unitDiscoverEventHandler, NULL);
  m_unitEvadeID = BWAPI::getUnitEvadeEvent().subscribe(&m_unitEvadeEventHandler, NULL);
  m_unitMorphID = BWAPI::getUnitMorphEvent().subscribe(&m_unitMorphEventHandler, NULL);
  m_unitRenegadeID = BWAPI::getUnitRenegadeEvent().subscribe(&m_unitRenegadeEventHandler, NULL);
  m_unitDestroyID = BWAPI::getUnitDestroyEvent().subscribe(&m_unitDestroyEventHandler, NULL);

  CurrentWorld::initialize();
}

PerceptualState::~PerceptualState()
{
  //SPAR_LOG(LogTypes.GENERAL_INFO, "~PerceptualState()");
  BWAPI::getUnitDiscoverEvent().unsubscribe(m_unitDiscoverID);
  BWAPI::getUnitEvadeEvent().unsubscribe(m_unitEvadeID);
  BWAPI::getUnitMorphEvent().unsubscribe(m_unitMorphID);
  BWAPI::getUnitRenegadeEvent().unsubscribe(m_unitRenegadeID);
  BWAPI::getUnitDestroyEvent().unsubscribe(m_unitDestroyID);

  std::map<size_t, UnitRecordWrapper>::iterator it = m_unitRecords.begin();
  while (it != m_unitRecords.end())
  {
    std::map<size_t, UnitRecordWrapper>::iterator next = ++(std::map<size_t, UnitRecordWrapper>::iterator(it));
    removeUnitWrapper(&it->second);
    it = next;
  }

  UnitRecordManager::terminate();

  CurrentWorld::terminate();
}

void PerceptualState::onFrameDebug()
{
#ifdef NON_FINAL_VERSION
  if (Config::getInstance().displayMap())
    displayMap();
  if (Config::getInstance().displayUnitsInFogOfWar())
    displayUnitsInFogOfWar();

  if (Config::getInstance().displayEnemyInfluenceMap())
    m_enemyThreatInfluenceMap.display();
  if (Config::getInstance().displaySelfInfluenceMap())
    m_selfThreatInfluenceMap.display();
  if (Config::getInstance().displayVisibilityInfluenceMap())
    m_visibilityInfluenceMap.display();
  //if (Config::getInstance().displayStructureInfluenceMap())
  //  m_structureInfluenceMap.display();
  if(Config::getInstance().displayDetectionInfluenceMap())
    m_detectionInfluenceMap.display();
  if(Config::getInstance().displayVulnerabilityInfluenceMap())
    m_vulnerabilityInfluenceMap.display();

  // Write unit orders
  //const std::set<BWAPI::Unit*>& units=BWAPI::Broodwar->self()->getUnits();
  //for(std::set<BWAPI::Unit*>::const_iterator it = units.begin(); it != units.end(); ++it)
  //{
  //  BWAPI::Unit* unit = *it;
  //  const int x = unit->getPosition().x();
  //  const int y = unit->getPosition().y();
  //  const char activeColor = '\x17';
  //  const std::string name = unit->getOrder().getName();
  //  BWAPI::Broodwar->drawTextMap(x, y, "%c%s", activeColor, name.c_str()); 
  //}
#endif
}

void PerceptualState::onStart()
{
  m_updateEnemyThreatInfluenceMap.start();
  m_updateSelfThreatInfluenceMap.start();
  m_updateVisibilityInfluenceMap.start();
  //m_updateStructuralImportanceInfluenceMap.start();
  m_updateDetectionInfluenceMap.start();
  m_updateVulnerabilityInfluenceMap.start();

  m_updateUnits.start();
}

void PerceptualState::onEnd()
{
}

void PerceptualState::onUnitRecordCreate(UnitRecord* unitRecord)
{
  addUnitWrapper(unitRecord);

  m_unitRecordCreateEvent.raise(unitRecord);
}

void PerceptualState::onUnitRecordDestroy(void* data)
{
  UnitRecordWrapper* wrapper = reinterpret_cast<UnitRecordWrapper*>(data);
  UnitRecord* unitRecord = wrapper->m_unitRecord;

  m_unitRecordDestroyEvent.raise(unitRecord);

  removeUnitWrapper(wrapper);
  // At this point unitRecord should be deleted... unless some other component keeps an history of the game
}

void PerceptualState::onUnitDiscover(void*, BWAPI::Unit* unit)
{
  // Put that in onUnitCreate?
  monitorUnitCompletion(unit, "Unit construction");
}

void PerceptualState::onUnitEvade(void*, BWAPI::Unit* unit)
{
  UnitRecord* unitRecord = getUnitRecord(unit);

  unitRecord->onUnitEvade();
}

void PerceptualState::onUnitDestroy(void*, BWAPI::Unit* unit)
{
  UnitRecord* unitRecord = getUnitRecord(unit);
  unitRecord->onUnitDiscover();

  unitRecord->destroy("BWAPI event");
}

void PerceptualState::onUnitMorph(void*, BWAPI::Unit* unit, BWAPI::UnitType previousType)
{
  unused(previousType);
  UnitRecord* unitRecord = getUnitRecord(unit);

  unitRecord->onUnitMorph();

  monitorUnitCompletion(unit, "Unit morph");
}

void PerceptualState::onUnitRenegade(void*, BWAPI::Unit* unit, BWAPI::Player* previousPlayer)
{
  unused(previousPlayer);
  UnitRecord* unitRecord = getUnitRecord(unit);

  unitRecord->onUnitRenegade();
}

TechMonitoringProcess* PerceptualState::onResearchStarted(TechTypeObject* tech, BWAPI::Unit* building)
{
  TechMonitoringProcess* research = new TechMonitoringProcess(this, building, tech);
  research->zombifyOnTerminate();
  research->start();
  return research;
}

UpgradeMonitoringProcess* PerceptualState::onUpgradeStarted(UpgradeTypeObject* upgrade, BWAPI::Unit* building)
{
  UpgradeMonitoringProcess* research = new UpgradeMonitoringProcess(this, building, upgrade);
  research->zombifyOnTerminate();
  research->start();
  return research;
}

BWAPI::Unit* getTrainingBuilding(BWAPI::Unit* unit)
{
  // This function deliberately does not appear in the .h so as to avoid tempting people to use it.

  BWAPI::Unit* building = NULL;
  const std::set<BWAPI::Unit*>& unitsOnTile = BWAPI::Broodwar->getUnitsOnTile(unit->getTilePosition().x(), unit->getTilePosition().y());
  for (std::set<BWAPI::Unit*>::const_iterator it = unitsOnTile.begin(); building == NULL && it != unitsOnTile.end(); ++it)
  {
    BWAPI::Unit* unitOnTile = *it;
    if (unitOnTile->getType() == unit->getType().whatBuilds().first)
      building = unitOnTile;
  }
  return building;
}

void PerceptualState::monitorUnitCompletion(BWAPI::Unit* unit, const char* description)
{
  if (unit->getPlayer() == BWAPI::Broodwar->self())
  {
    std::pair<stdext::hash_map<BWAPI::Unit*, MonitoringProcess*>::iterator, bool> result = 
      m_unitProcesses.insert(std::make_pair(unit, (MonitoringProcess*)NULL));
    if (result.second)
    {
      MonitoringProcess* process = NULL;
      BWAPI::Unit* trainingBuilding = NULL;
      if (!unit->getType().isBuilding() 
        && unit->getType().whatBuilds().first.isBuilding()
        && (trainingBuilding = getTrainingBuilding(unit)) != NULL)
      {
        process = new UnitTrainingMonitoringProcess(this, trainingBuilding, unit, description);
      }
      else
      {
        process = new UnitCreationMonitoringProcess(this, unit, description);
      }

      result.first->second = process;
      process->m_posInContainer = result.first;
      process->getTerminateEvent().subscribe(&m_unitProcessTerminateEventHandler, process);
      process->zombifyOnTerminate();
      process->start();      
    }
  }
}

bool PerceptualState::isPositionReachable(BWAPI::Position source, BWAPI::Position destination) const
{
  //longer for debug
  BWAPI::TilePosition tilePos(destination);
  bool valid = destination.isValid();
  bool hasPath = source.hasPath(destination);
 // bool isWalkable = BWAPI::Broodwar->isWalkable(position.x()/8, position.y()/8);
  std::set<BWAPI::Unit*> onTile = BWAPI::Broodwar->getUnitsOnTile(tilePos.x(), tilePos.y());
  std::set<BWAPI::Unit*> geysers = BWAPI::Broodwar->getGeysers();
  bool occupiedByGeyser = false;
  for(std::set<BWAPI::Unit*>::iterator it = geysers.begin(); it != geysers.end(); ++it)
  {
    if(tilePos.x() >= (*it)->getTilePosition().x() && tilePos.x() < (*it)->getTilePosition().x() + 4 && //geysers are 4 tiles by 2 tiles
      tilePos.y() >= (*it)->getTilePosition().y() && tilePos.y() < (*it)->getTilePosition().y() + 2){
       occupiedByGeyser = true;
       break;
    }
  }

  if(!valid || !hasPath || !onTile.empty() || occupiedByGeyser)
    return false;
  /*
   if(!position.isValid() ||
     !unit->hasPath(destination) ||
     !BWAPI::Broodwar->isWalkable(destination.x()/8, destination.y()/8) ||
     !BWAPI::Broodwar->getUnitsOnTile(destination.x()/TILE_SIZE, destination.y()/TILE_SIZE).empty())
     return false;*/
   return true;
}

std::set<BWAPI::Unit*> PerceptualState::getTargetingUnits(BWAPI::Unit* unit) const
{
  class IsTargeting : public std::unary_function<BWAPI::Unit*, bool>
  {
    BWAPI::Unit* m_target;
  public:
    IsTargeting(BWAPI::Unit* target) : m_target(target) {}
    bool operator()(BWAPI::Unit* unit)
    {
      return unit->getOrderTarget() == m_target;
    }
  };

  // Note: does not include enemy bunkers

  std::set<BWAPI::Unit*> targetingUnits;

  const std::set<BWAPI::Unit*>& enemyUnits = BWAPI::Broodwar->enemy()->getUnits();
  std::remove_copy_if(
    enemyUnits.begin(), enemyUnits.end(),
    std::inserter(targetingUnits, targetingUnits.begin()),
    IsTargeting(unit));

  return targetingUnits;
}

bool PerceptualState::isUnitTargeted(BWAPI::Unit* unit) const
{
  // Note: does not include enemy bunkers

  bool isTargeted = false;
  const std::set<BWAPI::Unit*>& enemyUnits = BWAPI::Broodwar->enemy()->getUnits();
  for (std::set<BWAPI::Unit*>::const_iterator it = enemyUnits.begin(); !isTargeted && it != enemyUnits.end(); ++it)
  {
    isTargeted = (*it)->getTarget() == unit;
  }
  return isTargeted;
}

BWAPI::Position PerceptualState::getGroundFrontier(BWAPI::Position position, const double angle) const
{
  BWAPI::Position frontier(position);

  BWTA::Region* region = BWTA::getRegion(BWAPI::TilePosition(position));
  if(region != NULL)
  {
    const BWTA::Polygon& polygon = region->getPolygon();

    if(!polygon.empty())
    {
      double min_dist = boost::numeric::bounds<double>::highest();
      Vector2f intersection(0, 0);

      const Vector2f point(position.x(), position.y());

      // TODO check other polygons (e.g., if intersection lies on a chokepoint)
      for(BWTA::Polygon::const_iterator prev=polygon.begin(), i=prev+1 /*skip first vertex*/;
        i!=polygon.end();
        ++i, ++prev)
      {
        const Vector2f p1(prev->x(), prev->y());
        const Vector2f p2(i->x(), i->y());

        const double distance = ::ray_segment_intersection(point, angle, p1, p2, intersection);
        if(distance >= 0.0 && distance < min_dist)
        {
          frontier.x() = static_cast<int>(intersection.x());
          frontier.y() = static_cast<int>(intersection.y());
          min_dist = distance;
        }
      }

      // check back()---front() segment as well
      {
        const Vector2f first(polygon.front().x(), polygon.front().y());
        const Vector2f last(polygon.back().x(), polygon.back().y());

        const double distance = ::ray_segment_intersection(point, angle, last, first, intersection);
        if(distance >= 0.0 && distance < min_dist)
        {
          frontier.x() = static_cast<int>(intersection.x());
          frontier.y() = static_cast<int>(intersection.y());
          // min_dist = distance;
        }
      }
    }
  }

  return frontier;
}

/**
* @brief Functor that acts as a goal test function for the breadth first search algorithm
*/
struct AtEnemyInfluenceBorder
{
  typedef bool return_type;

  bool operator()(size_t x, size_t y, const InfluenceMap<Threat> & map) const
  {
    const Threat& threat = map.getData(x, y);
    return threat.isNull() != m_current;
  }

  AtEnemyInfluenceBorder(bool current) : m_current(current) {}

private:
  bool m_current;
};

BWAPI::Position PerceptualState::getEnemyInfluenceBorder(BWAPI::Position position) const
{
  InfluenceMapRef<Threat>::map_ptr p = m_enemyThreatInfluenceMap.get();
  std::pair<int,int> pos = InfluenceMaps::breadth_first_search<AtEnemyInfluenceBorder, Threat>(
    position.x() / TILE_SIZE,
    position.y() / TILE_SIZE,
    AtEnemyInfluenceBorder(p->getData(position).isNull()),
    p);

  return BWAPI::Position(pos.first * TILE_SIZE, pos.second * TILE_SIZE);
}

struct ThreatGradient
{
  typedef std::pair<double,double> return_type;

  ThreatGradient(bool ground) : m_ground(ground) {}

  return_type operator()(int x, int y, const InfluenceMap<Threat> & selfMap, const InfluenceMap<Threat> & enemyMap)
  {
    ThreatGradientEvaluator evaluator(selfMap, enemyMap, m_ground, x, y);
    return BresenhamCircle(evaluator, 10, true).getResult();
  }

private:
  bool m_ground;

  struct ThreatGradientEvaluator : std::binary_function<int,int,void>
  {
    ThreatGradientEvaluator(const InfluenceMap<Threat> & selfMap, const InfluenceMap<Threat> & enemyMap, bool ground, int x, int y)
      : m_self(selfMap), m_enemy(enemyMap), m_ground(ground), m_x(x), m_y(y), m_vx(0.0), m_vy(0.0) {}

    void operator()(int x, int y)
    {
      if (m_self.isValid(x + m_x, y + m_y) && !(x == 0 && y == 0))
      {
        const Threat& threat = m_enemy.getData(x + m_x, y + m_y);
        const Threat& self   = m_self.getData( x + m_x, y + m_y);

        double value = m_ground ?
          threat.ground - self.ground:
          threat.air    - self.air;

        // Penalties
        if (x + m_x == 0 || x + m_x == m_self.getWidth()  - 1) value += 100.0;
        if (y + m_y == 0 || y + m_y == m_self.getHeight() - 1) value += 100.0;

        //if(!Spar->getPerceptualState()->isPositionReachable(BWAPI::Position(x * TILE_SIZE, y * TILE_SIZE), BWAPI::Position((x + m_x) * TILE_SIZE, (y + m_y) * TILE_SIZE)))
        if (m_ground && !BWAPI::Broodwar->isWalkable(x + m_x, y + m_y))
          value += 100.0;

        double norm = std::sqrt(static_cast<double>(x*x + y*y));

        m_vx += value * x/norm;
        m_vy += value * y/norm;
      }
    }

    return_type getResult() const
    {
      double norm = std::sqrt(m_vx*m_vx + m_vy*m_vy);
      return return_type(m_vx/norm, m_vy/norm);
    }

  private:
    const InfluenceMap<Threat> & m_self;
    const InfluenceMap<Threat> & m_enemy;
    const bool m_ground;
    const int m_x;
    const int m_y;

    double m_vx;
    double m_vy;

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

struct StopCriterion
{
  StopCriterion(int max_iterations) : m_max(max_iterations) {}

  bool operator()(const std::pair<double,double>& gradient)
  {
    if (m_max-- <= 0) return true;

    double sqnorm = gradient.first*gradient.first + 
      gradient.second*gradient.second;
    return sqnorm < 1e-3;
  }

private:
  int m_max;
};

BWAPI::Position PerceptualState::getSafeRetreatPosition(BWAPI::Unit* unit) const
{
  std::pair<int,int> pos = InfluenceMaps::gradient_descent<ThreatGradient, StopCriterion, Threat, Threat>(
    unit->getTilePosition().x(),
    unit->getTilePosition().y(),
    ThreatGradient(!unit->getType().isFlyer()),
    StopCriterion(5),
    m_selfThreatInfluenceMap.get(),
    m_enemyThreatInfluenceMap.get());

  SPAR_LOG(LogTypes.PERCEPTUAL_STATE, "Start: (%d,%d)\tDestination: (%d,%d)", unit->getTilePosition().x(), unit->getTilePosition().y(), pos.first, pos.second);
  return BWAPI::Position(pos.first * TILE_SIZE, pos.second * TILE_SIZE);
}

//BWAPI::Position PerceptualState::getSafeRetreatPosition(const PersistentUnitGroup* group, const std::set<BWAPI::Unit*>& enemyUnits) const
//{
//  // Delete eventually! retreat positions should be assigned to clusters of units instead of entire group!
//
//  BWAPI::Position enemiesVector(0, 0);
//  for (std::set<BWAPI::Unit*>::const_iterator it = enemyUnits.begin();
//    it != enemyUnits.end();
//    ++it)
//  {
//    for (PersistentUnitGroup::const_iterator itUnit = group->begin();
//      itUnit != group->end();
//      ++itUnit)
//    {
//      enemiesVector += ((*it)->getPosition() - (*itUnit)->getPosition());
//    }
//  }
//  const double destinationAngle = atan2(static_cast<double>(-enemiesVector.y()),
//    static_cast<double>(-enemiesVector.x()));
//  BWAPI::Position center = group->getCenter();
//  BWAPI::Position destination = this->getGroundFrontier(center, destinationAngle);
//
//  return destination;
//}

void PerceptualState::displayMap()
{
  //we will iterate through all the base locations, and draw their outlines.
  for(std::set<BWTA::BaseLocation*>::const_iterator i=BWTA::getBaseLocations().begin();i!=BWTA::getBaseLocations().end();i++)
  {
    BWAPI::TilePosition p=(*i)->getTilePosition();
    BWAPI::Position c=(*i)->getPosition();

    //draw outline of center location
    BWAPI::Broodwar->drawBox(BWAPI::CoordinateType::Map,p.x()*32,p.y()*32,p.x()*32+4*32,p.y()*32+3*32,BWAPI::Colors::Blue,false);

    //draw a circle at each mineral patch
    for(std::set<BWAPI::Unit*>::const_iterator j=(*i)->getStaticMinerals().begin();j!=(*i)->getStaticMinerals().end();j++)
    {
      BWAPI::Position q=(*j)->getInitialPosition();
      BWAPI::Broodwar->drawCircle(BWAPI::CoordinateType::Map,q.x(),q.y(),30,BWAPI::Colors::Cyan,false);
    }

    //draw the outlines of vespene geysers
    for(std::set<BWAPI::TilePosition>::const_iterator j=(*i)->getGeysersPosition().begin();j!=(*i)->getGeysersPosition().end();j++)
    {
      BWAPI::TilePosition q=*j;
      BWAPI::Broodwar->drawBox(BWAPI::CoordinateType::Map,q.x()*32,q.y()*32,q.x()*32+4*32,q.y()*32+2*32,BWAPI::Colors::Orange,false);
    }

    //if this is an island expansion, draw a yellow circle around the base location
    if ((*i)->isIsland())
    {
      BWAPI::Broodwar->drawCircle(BWAPI::CoordinateType::Map,c.x(),c.y(),80,BWAPI::Colors::Yellow,false);
    }
  }

  //we will iterate through all the regions and draw the polygon outline of it in green.
  for(std::set<BWTA::Region*>::const_iterator r=BWTA::getRegions().begin();r!=BWTA::getRegions().end();r++)
  {
    BWTA::Polygon p=(*r)->getPolygon();
    for(int j=0;j<(int)p.size();j++)
    {
      BWAPI::Position point1=p[j];
      BWAPI::Position point2=p[(j+1) % p.size()];
      BWAPI::Broodwar->drawLine(BWAPI::CoordinateType::Map,point1.x(),point1.y(),point2.x(),point2.y(),BWAPI::Colors::Green);
    }
  }

  //we will visualize the chokepoints with red lines
  for(std::set<BWTA::Region*>::const_iterator r=BWTA::getRegions().begin();r!=BWTA::getRegions().end();r++)
  {
    for(std::set<BWTA::Chokepoint*>::const_iterator c=(*r)->getChokepoints().begin();c!=(*r)->getChokepoints().end();c++)
    {
      BWAPI::Position point1=(*c)->getSides().first;
      BWAPI::Position point2=(*c)->getSides().second;
      BWAPI::Broodwar->drawLine(BWAPI::CoordinateType::Map,point1.x(),point1.y(),point2.x(),point2.y(),BWAPI::Colors::Red);
    }
  }
}

void PerceptualState::displayUnitsInFogOfWar()
{
  std::pair<Map::unit_const_iterator, Map::unit_const_iterator> neutralUnits = m_map.getNeutralUnits();
  for (; neutralUnits.first != neutralUnits.second; ++neutralUnits.first)
  {
    const UnitRecord* unitRecord = *neutralUnits.first;
    if (!unitRecord->isAccessible())
    {
      BWAPI::UnitType type = unitRecord->getType();
      BWAPI::Position pos = unitRecord->getPosition();
      BWAPI::Broodwar->drawBoxMap(pos.x() - type.dimensionLeft(), pos.y() - type.dimensionUp(), 
                                  pos.x() + type.dimensionLeft(), pos.y() + type.dimensionUp(),
                                  BWAPI::Colors::Grey);
    }
  }

  std::pair<Map::unit_const_iterator, Map::unit_const_iterator> enemyUnits = m_map.getEnemyUnits();
  for (; enemyUnits.first != enemyUnits.second; ++enemyUnits.first)
  {
    const UnitRecord* unitRecord = *enemyUnits.first;
    if (!unitRecord->isAccessible())
    {
      BWAPI::UnitType type = unitRecord->getType();
      BWAPI::Position pos = unitRecord->getPosition();
      BWAPI::Broodwar->drawBoxMap(pos.x() - type.dimensionLeft(), pos.y() - type.dimensionUp(), 
                                  pos.x() + type.dimensionLeft(), pos.y() + type.dimensionUp(),
                                  BWAPI::Colors::Red);
      BWAPI::Broodwar->drawTextMap(pos.x(), pos.y(), "%c%s", '\x17', unitRecord->getType().getName().c_str());

    }
  }
}

double ray_segment_intersection(const Vector2f& point, const double angle, const Vector2f& p1, const Vector2f& p2, Vector2f& intersection)
{
  const Vector2f segment = p2 - p1;
  const Vector2f ray(cos(angle), sin(angle));

  const Vector2f psegment = segment.perpendicular();
  const Vector2f pray = ray.perpendicular();

  // Make sure that lines are not parallel
  const float d = psegment.dot(ray);
  if(d != 0.f)
  {
    const float g = psegment.dot(p1 - point) / d;
    const float h = pray.dot(p1 - point) / d;

    if(g >= 0 && 0 <= h && h <= 1)
    {
      intersection = point + g * ray;
      const double distance = g; // ray is a unit-length vector

      //#ifdef _DEBUG
      //        Vector2f intersection2 = p1 + h*segment;
      //        double distance2 = point.distance_to(intersection);
      //
      //        assert(intersection.almost_equal_to(intersection2));
      //        assert(fabs(distance - distance2) < 1e-3);
      //#endif

      return distance;
    }
  }

  return -1.0;
}
