EGraphPlayerIF.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------


#ifndef EGRAPH_PLAYER_IF_H
#define EGRAPH_PLAYER_IF_H

#include <libplayerc++/playerc++.h>
#include <vector>
#include <list>
#include <unistd.h>
#include <math.h>
#include <cstdlib>
#include <assert.h>
#include <iostream>
#include <fstream>

// MIP
#include <R2.h>
#include <Grid.h>
#include <Scan.h>

#include "args.h"
#include "utils.h"

using namespace PlayerCc;
using namespace std;
using namespace boost;
using namespace MipBaselib;

enum BehaviorTypes
{
 EXPAND,
 GUARD,
 FOLLOW,
 WANDER,
 NO_BEHAVIOR
};

enum KeepColor
{
 KEEP_UNKNOWN,
 KEEP_ORANGE,
 KEEP_PURPLE
};

class PursuitAgent;

typedef uint Identity;
typedef std::vector<Pose > Path;
typedef std::vector<PursuitAgent* > AgentVector;
typedef std::vector<Pose > ViewPoints;

ofstream gLogFile;
Timer gTotalTimer;
int gIterCount;
Grid *gGlobalGrid;
AgentVector gAgentVector;

PlayerClient* gPC;
Graphics2dProxy* gGP = NULL;
MapProxy* gMP = NULL;

int gMapWidth = -1;
int gMapHeight = -1;
double gMapResolution = -1;

int gMinArcSize = 2;
int gMinMergeArcSize = 1;
int gMinRadialArcSize = 3;
double gSafeRangeFrac = 0.92;
double gSafeAngleFrac = 0.92;
double gCoverDistScalar = 1.0;
double gOverlapFactor = 1.08;

Pose adjustViewPoint( Pose initialPose ){
 // Force viewpoints to be on 5cm x 5cm grid, which forces grids to mostly align.
 
 //int iX = (int)round( 20*initialPose.pos().x() );
 //int iY = (int)round( 20*initialPose.pos().y() );
 
 //return Pose( iX/20.0, iY/20.0, initialPose.ori() );
 
 return initialPose;
}

void drawSquare( player_color_t color, Position pos, double border, bool bFill ){
 player_point_2d_t square[4], center;
 
 center.px = pos.x();
 center.py = pos.y();
 
 square[0].px = center.px - border;
 square[0].py = center.py - border;
 square[1].px = center.px + border;
 square[1].py = center.py - border;
 square[2].px = center.px + border;
 square[2].py = center.py + border;
 square[3].px = center.px - border;
 square[3].py = center.py + border;
 
 gGP->Color( color );
 gGP->DrawPolygon( square, 4, bFill, color );
}

void drawX( player_color_t color, Position pos, double width ){
 player_point_2d_t square[4], center;
 
 center.px = pos.x();
 center.py = pos.y();
 
 square[0].px = center.px - width;
 square[0].py = center.py - width;
 square[2].px = center.px + width;
 square[2].py = center.py - width;
 square[1].px = center.px + width;
 square[1].py = center.py + width;
 square[3].px = center.px - width;
 square[3].py = center.py + width;
 
 gGP->Color( color );
 gGP->DrawPolyline( square, 2 );
 gGP->DrawPolyline( &(square[2]), 2 );
}

void drawRedX( Position pos, double width ){
 player_color_t color;
 
 color.alpha = 255;
 color.red = 255;
 color.green = 0;
 color.blue = 0;
 
 drawX( color, pos, width);
}


class PursuitAgent{
 
protected :
 Identity m_iID;
 PlayerClient* m_pClient;
 Position2dProxy* m_pPosProxy;
 LaserProxy* m_pLaserProxy;
 
 BehaviorTypes m_eBehavior;
 Frontier m_frontier;
 Frontier m_freeArcs;
 Frontier m_prevFrontier;
 Grid m_LSR;
 Grid m_prevLSR;
 bool m_bFrontierInitialized;
 Path m_path;
 PursuitAgent* m_pWaitForAgent;
 AgentVector m_followers;
 
 // Agent action functions
 void Expand( AgentVector neighbors );
 void Guard( AgentVector neighbors );
 void processFrontier( AgentVector neighbors );
 void Wander( AgentVector neighbors );
 
 void recordLSR();
 //bool isCanExpandAlone();
 bool isCellCovered( Pose viewPoint, DubInt iCell, bool bSilent = true );
 
 //
 // These next functions are the two main functions for the algorithm
 //
 
 // Pick a viewpoint on current frontier to expand
 Pose pickBestViewPoint(int *iNumViewPoints, bool bDebugDisplay = false);
 
 // Update frontier after arriving at new viewpoint, involves clearing
 // some of neighbors frontier arcs and classifying LSR boundary
 void updateFrontier( AgentVector neighbors, bool bDebugDisplay = false );
 void getNeighborFrontiers( AgentVector neighbors, Pose &curPose, vector<Frontier > &ourNeighborFrontiers, 
   vector<Frontier > &updatedNeighborFrontiers,
   vector<DubInt > &neighborObstacleEndpoints,
   vector<DubInt > &neighborObstacleBeginpoints );
 void classifyOurFrontier( AgentVector neighbors, Pose &curPose, vector<Frontier > &ourNeighborFrontiers, 
   vector<DubInt > &neighborObstacleEndpoints,
   vector<DubInt > &neighborObstacleBeginpoints,
   bool bDebugDisplay );
 void updateNeighborFrontiers( AgentVector neighbors, Pose &curPose, vector<Frontier > &updatedNeighborFrontiers );
 void postProcessOurFrontier( vector<Frontier > &ourNeighborFrontiers, Pose &curPose, bool bDebugDisplay );
 
 
public :
 PursuitAgent( Identity ID, PlayerClient* client, Position2dProxy* posProxy, LaserProxy* laserProxy );
 ~PursuitAgent( );
 
 void clientRead();
 Identity getID(){ return m_iID; }
 BehaviorTypes getBehavior(){ return m_eBehavior; }
 Grid getLSR(){ return ((m_bFrontierInitialized) ? m_LSR : m_prevLSR); }
 LRR getLRR(){ return ((m_bFrontierInitialized) ? m_LSR._lrr : m_prevLSR._lrr); }
 Frontier getFrontier(){ return ((m_bFrontierInitialized) ? m_frontier : m_prevFrontier); }
 
 int getNumFrontierSegments(){ return ((m_bFrontierInitialized) ? m_frontier.size() : m_prevFrontier.size()); }
 int getTotalFrontierCells();
 void setFrontier(Frontier frontier, bool bForce = false );
 Path getPath(){ return m_path; }
 Pose getPose(){ clientRead(); return Pose(m_pPosProxy->GetXPos(), m_pPosProxy->GetYPos(), m_pPosProxy->GetYaw()); }
 Position getPosition(){ return getPose().pos(); }
 Pose getViewpoint();
 Scan getScan();
 ExplorationParams getExplParams();
 
 double getSensorRange(){ return m_pLaserProxy->GetMaxRange(); }
 double getCoverRange();
 double getCoverFieldOfView();
 double getWidth(){ return m_pPosProxy->GetSize().sw; }
 int getLSRSize(){ return ((m_bFrontierInitialized) ? m_LSR.columns()*m_LSR.rows() : m_prevLSR.columns()*m_prevLSR.rows()); }
 vector<Cell* > getRaster( DubInt iCell1, DubInt iCell2 );
 double getLSRResolution(){ return ((m_bFrontierInitialized) ? m_LSR.resolution() : m_prevLSR.resolution()); }
 Pose getLSRPose();
 LSRBound getLSRBound(){ return ((m_bFrontierInitialized) ? m_LSR.lsrBound() : m_prevLSR.lsrBound()); }
 Cell* getCell( DubInt indices ){ return ((m_bFrontierInitialized) ? m_LSR.getCell(indices) : m_prevLSR.getCell(indices)); }
 Cell* getCell( Position pos ){ return ((m_bFrontierInitialized) ? m_LSR.getCell(pos) : m_prevLSR.getCell(pos)); }
 Cell* getCell( int i ){ return ((m_bFrontierInitialized) ? m_LSR.getCell(i) : m_prevLSR.getCell(i)); }
 Position getCellPosition( DubInt indices ){ return getCell(indices)->point(); }
 int getNumFollowers(){ return m_followers.size(); }
 Position getFollowerPosition();
 
 // State change functions
 void setToExpand( Path path, bool bClearPrevFrontier, AgentVector neighbors, bool bDoExpand = true );
 void setToGuard( AgentVector neighbors );
 void setToFollow( AgentVector neighbors );
 void setToWander( AgentVector neighbors );
 void setToNoBehavior( );
 
 void goTo( Pose pose );
 void addFollower( PursuitAgent* newFollower, bool bMove = true );
 
 // This function is called whenever agent is active
 void doCurrentBehavior( AgentVector neighbors );
 
 bool isLeader();
 bool isReady();
 bool isWaitingForAnyAgent();
 bool isWaitingForAgent(int iID);
 void clearWaitingForAgent();
 bool isTraveling();
 
 void drawBoundary( Graphics2dProxy &gp );
 void drawFrontier( Graphics2dProxy &gp );
 void drawLSR( Graphics2dProxy &gp, bool bOnlyLSR );
 
 bool canDetectTargetPosition( Position targetPos );
};

// For logging status of algorithm
void logData( ){
 double time = gTotalTimer.get().dCast();
 int iAreaExposed = 0.0;
 
 if( gGlobalGrid != NULL )
 {
  iAreaExposed = gGlobalGrid->_lsr.size();
 }
 
 int iFrontierCount = 0;
 int iGuard = 0;
 int iExpand = 0;
 int iFollow = 0;
 int iWander = 0;
 for( int i = 0; i < gAgentVector.size(); i++ )
 {
  switch (gAgentVector[i]->getBehavior()) 
  {
   case EXPAND:
    iExpand++;
    iFrontierCount += gAgentVector[i]->getTotalFrontierCells();
    break;
   case GUARD:
    iGuard++;
    iFrontierCount += gAgentVector[i]->getTotalFrontierCells();
    break;
   case FOLLOW:
    iFollow++;
    break;
   case WANDER:
    iWander++;
    break;
   default:
    break;
  }
 }
 
 gLogFile << gIterCount << ", ";
 gLogFile << time << ", ";
 gLogFile << iAreaExposed << ", ";
 gLogFile << iFrontierCount << ", ";
 gLogFile << iExpand << ", ";
 gLogFile << iGuard << ", ";
 gLogFile << iFollow << ", ";
 gLogFile << iWander;
 
 gLogFile << endl;
}

PursuitAgent::PursuitAgent( Identity ID, PlayerClient* client, Position2dProxy* posProxy, LaserProxy* laserProxy ){
 m_iID = ID;
 m_pClient = client;
 m_pPosProxy = posProxy;
 m_pLaserProxy = laserProxy;
 
 m_eBehavior = NO_BEHAVIOR;
 m_frontier.clear();
 m_freeArcs.clear();
 m_prevFrontier.clear();
 m_LSR = Grid();
 m_prevLSR = Grid();
 m_bFrontierInitialized = false;
 m_path.clear();
 m_pWaitForAgent = NULL;
 m_followers.clear();
 
 // Set up position proxy
 m_pPosProxy->SetMotorEnable(true);
 m_pPosProxy->RequestGeom();
 
 while( m_pLaserProxy->GetMaxRange() <= 0.0 || m_pLaserProxy->GetCount() <= 0 )
 {
  //if( gDebug > 9 ) cout<< "  waiting for real data ..." << endl;
  clientRead();
 }
}

void PursuitAgent::Expand( AgentVector neighbors ){
 if( gDebug > 5 ) cout<< "Agent " << getID() << ":  doing EXPAND to " << m_path.back().print() << endl;
 
 if( gDebug > 8 ) 
 {
  cout<< "  " << getID() << " has followers: ";
  for( int i = 0; i < m_followers.size(); i++ )
  {
   cout<< m_followers[i]->getID() << ", ";
  }
  cout<< endl;
 }
 
 if( isTraveling() )
 {
  if( gDebug > 5 ) cout<< "  " << getID() << ":  waiting to reach path pose " << m_path.back().print() << endl;
  return;
 }
 
 AgentVector::iterator i;
 int iCount = 0;
 for(i = m_followers.begin(); i != m_followers.end(); i++ )
 {
  if( getPosition().dist((*i)->getPosition()) > getSensorRange() )
  {
   if( gDebug > 5 ) cout<< getID() << ": follower " << (*i)->getID() << " still needs to catch up" << endl;
   Position followerPosition = getFollowerPosition();
   followerPosition *= (Decimal)(iCount+1.0);
   (*i)->goTo( Pose(getPosition() - followerPosition, m_path.back().ori()) );
  }
  iCount++;
 }
 
 //recordLSR();
 
 setToGuard( neighbors );
}

void PursuitAgent::Guard( AgentVector neighbors )
{
 if( gDebug > 5 ) cout<< "Agent " << getID() << ":  doing GUARD" << endl;
 if( gDebug > 8 ) 
 {
  cout<< "  " << getID() << " has followers: ";
  for( int i = 0; i < m_followers.size(); i++ )
  {
   cout<< m_followers[i]->getID() << ", ";
  }
  cout<< endl;
 }
 
 // Send excess followers to nearby guards
 if( m_bFrontierInitialized && getNumFollowers() > 2 )
 {
  Position curPos = getLSRPose().pos();
  Frontier ourFrontier = m_LSR.frontier();
  
  vector<PursuitAgent* > nearbyLeaders;
  
  for( int i = 0; i < neighbors.size(); i++ )
  {
   if( neighbors[i]->isLeader() && neighbors[i]->getID() != getID() )
   {
    if( neighbors[i]->getNumFollowers() < getNumFollowers() / 2 )
    {
     if( curPos.dist( neighbors[i]->getPosition() ) < 2.0 * getSensorRange() )
     {
      bool bClose = false;
    
      for( int m = 0; (!bClose && m < ourFrontier.size()); m++ )
      {
       for( int n = 0; (!bClose && n < ourFrontier[m].size()); n++ )
       {
        Position cellPos = curPos + m_LSR.getCell(ourFrontier[m][n])->point() - neighbors[i]->getPosition();
        Cell* pTheirCell = neighbors[i]->getCell(cellPos);
        
        if( pTheirCell != NULL && pTheirCell->isLSR() )
        {
         nearbyLeaders.push_back(neighbors[i]);
        }
       }
      }
     }
    }
   }
  }
  
  if( nearbyLeaders.size() > 0 )
  {
   int iFollowersToGive = getNumFollowers() / 2;
   
   while( iFollowersToGive > 0 )
   {
    for( int i = 0; i < nearbyLeaders.size(); i++ )
    {
     if( iFollowersToGive > 0 )
     {
      iFollowersToGive--;
      
      if( gDebug > 2 ) cout<< "  " << getID() << " giving " << nearbyLeaders[i]->getID() << " follower " << m_followers.back()->getID() << endl;
      
      nearbyLeaders[i]->addFollower( m_followers.back(), true );
      m_followers.pop_back();
     }
    }
   }
  }
 }
 if( m_bFrontierInitialized )
 {
  Position curPos = getLSRPose().pos();
  for( int i = 0; i < m_followers.size(); i++ )
  {
   Cell* pCell = m_LSR.getCell( m_followers[i]->getPosition() - curPos );
   
   if( pCell == NULL || !(pCell->isLSR()) )
   {
    // release follower who is too far away
    m_followers[i]->setToWander( neighbors );
    m_followers.erase(m_followers.begin() + i);
    i--;
   }
  }
 }
 
 
 if( isWaitingForAnyAgent() )
 {
  if( gDebug > 5 ) cout<< "  " << getID() << ":  is waiting for agent " << m_pWaitForAgent->getID() << endl;
  return;
 }
 
 // Update frontier and neighbor's frontiers, only really does anything
 // when agent first appears at new viewpoint
 updateFrontier( neighbors, (gDebug > 8) );
 
 // 
 processFrontier( neighbors );
}

void PursuitAgent::processFrontier( AgentVector neighbors )
{
 if( getTotalFrontierCells() < gMinArcSize )
 {
  if( m_pWaitForAgent != NULL )
  {
   m_pWaitForAgent->setToWander(neighbors);
  }
  
  if( gDebug > 0 ) cout<< getID() << " has only " << getTotalFrontierCells() << " frontier cells, switching to wander" << endl;
  
  m_frontier.clear();
  m_prevFrontier.clear();
  
  setToWander(neighbors);
  return;
 }
 
 int iNumViewPoints;
 // Pick single best view point to expand current frontier
 Pose bestViewPoint = pickBestViewPoint(&iNumViewPoints, (gDebug > 30));
 
 if( bestViewPoint == Pose(0,0,(Angle)0) )
 {
  if( iNumViewPoints == 1 || m_followers.size() > 0 )
  {
   // Error condition, happens when stage screws up and produces arcs in impossible places.
   //cout<< "WARNING:  best viewpoint for " << getID() << " set to current pose!" << endl;
   
   //cout<< getID() << " switching to WANDER!" << endl;
   //if( m_pWaitForAgent != NULL )
   //{
    //m_pWaitForAgent->setToWander();
   //}
   
   //setToWander();
   return;
  }
 }
 
 bestViewPoint = Pose( bestViewPoint.pos() + getLSRPose().pos(), bestViewPoint.ori() );
 //if( gDebug > 8 ) cout<< "    Best view point (" << bestViewPoint.x() << "," << bestViewPoint.y() << ")" << endl;
 bestViewPoint = adjustViewPoint( bestViewPoint );
 if( gDebug > 8 ) cout<< "    Best view point (" << bestViewPoint.print() << ")" << endl;
 
 // Optimal path to new viewpoint is straight line from current VP to new point,
 // never a need to avoid obstacles
 Path pathToBestViewPoint;
 pathToBestViewPoint.push_back( getPose() );
 pathToBestViewPoint.push_back( bestViewPoint );
 
 if( m_pWaitForAgent != NULL )
 {
  // Re-route agent who is already expanding, desired new view point might have changed after
  // part of our frontier was cleared
  m_pWaitForAgent->setToExpand( pathToBestViewPoint, true, neighbors );
 }
 else if( iNumViewPoints == 1 )
 {
  if( gDebug > 8 ) cout<< "    Expanding alone" << endl;
  
  // Agent decides it can expand alone.  For experiments, may not want to do this depending on
  // how mutual localization is handled.
  
  // Save frontier, LSR for this point so that we can use it to determine new frontier when
  // arriving at new location
  if( m_bFrontierInitialized )
  {
   m_prevFrontier = m_frontier;
   m_prevLSR = m_LSR;
  }
  
  setToExpand( pathToBestViewPoint, false, neighbors );
  
  // TODO: auction off extra followers?
 }
 else
 {
  if( m_followers.size() > 0 )
  {
   if( gDebug > 8 ) cout<< "    Sending follower " << m_followers.back()->getID() << endl;
   
   // TODO: what to do about far away followers who may not be close to current viewpoint?
   
   PursuitAgent* pExpander = m_followers.front();
   m_pWaitForAgent = pExpander;
   pExpander->setToExpand( pathToBestViewPoint, true, neighbors );
   m_followers.erase(m_followers.begin());
   
   int iMaxNumFollowersToGive = std::min( m_followers.size(), (m_followers.size() + 1)/(iNumViewPoints) );
   
   if( gDebug > 8 ) cout<< "    Giving follower " << iMaxNumFollowersToGive << " followers out of " << m_followers.size() << endl;
   for( int i = 0; i < iMaxNumFollowersToGive; i++ )
   {
    pExpander->addFollower( m_followers.back() );
    m_followers.pop_back();
   }
  }
  else
  {
   // Wait for followers to join us, or for someone else to clear part of our frontier
  }
  
  // This agent will keep doing guard behavior on next iteration
 }
}

void PursuitAgent::Wander( AgentVector neighbors )
{
 if( gDebug > 5 ) cout<< "Agent " << getID() << " doing WANDER" << endl;
 AgentVector leaders;
 vector<Pose > leaderPaths;
 AgentVector allLeaders;
 AgentVector wanderers;
 
 recordLSR();
 Frontier ourFrontier = m_LSR.frontier();
 
 Position curPos = getPosition();
 
 // Create list of nearby agents of different types
 for( int i = 0; i < neighbors.size(); i++ )
 {
  if( neighbors[i]->isLeader() && neighbors[i]->getID() != getID() )
  {
   allLeaders.push_back(neighbors[i]);

   if( curPos.dist( neighbors[i]->getPosition() ) < 2.0*getSensorRange() )
   {
    bool bClose = false;
    
    for( int m = 0; (!bClose && m < ourFrontier.size()); m++ )
    {
     for( int n = 0; (!bClose && n < ourFrontier[m].size()); n++ )
     {
      Position cellPos = curPos + m_LSR.getCell(ourFrontier[m][n])->point() - neighbors[i]->getLSRPose().pos();
      Cell* pTheirCell = neighbors[i]->getCell(cellPos);
      
      if( pTheirCell != NULL && pTheirCell->isLSR() )
      {
       if( gDebug > 8 )
       {
        drawSquare( gColorBlue, curPos + m_LSR.getCell(ourFrontier[m][n])->point(), 0.05, false );
        usleep( 300000 );
       }
       leaders.push_back(neighbors[i]);
       leaderPaths.push_back( Pose(curPos + m_LSR.getCell(ourFrontier[m][n])->point(), Angle(M_PI) + pTheirCell->point().bearing()));
       bClose = true;
      }
     }
    }
   }
  }
  else if( neighbors[i]->getBehavior() == WANDER )
  {
   if( neighbors[i]->getID() != getID() )
   {
    if( curPos.dist( neighbors[i]->getPosition() ) < getSensorRange() )
    {
     Cell* pCell = m_LSR.getCell(neighbors[i]->getPosition() - curPos);
     if( pCell != NULL && pCell->isLSR() )
     {
      wanderers.push_back(neighbors[i]);
     }
    }
   }
  }
 }
 
 // Look for new leader to follow
 if( leaders.size() > 0 )
 {
  int iNewLeader = -1;
  int iMinFollowers = 10;
  
  for( int i = 0; i < leaders.size(); i++ )
  {
   int iFollowers = leaders[i]->getNumFollowers() + (leaders[i]->isWaitingForAnyAgent() ? 1 : 0);
   
   if( (iFollowers < iMinFollowers) ||
    ((iNewLeader >= 0) && (iFollowers == iMinFollowers) && (getPosition().dist(leaders[i]->getPosition()) < getPosition().dist(leaders[iNewLeader]->getPosition()))) )
   {
    iNewLeader = i;
    iMinFollowers = iFollowers;
   }
  }
  
  if( iNewLeader >= 0 )
  {
   Cell* pOurCell = m_LSR.getCell( leaders[iNewLeader]->getPosition() - curPos );
   Cell* pTheirCell = leaders[iNewLeader]->getCell( curPos - leaders[iNewLeader]->getPosition() );
   if( (pOurCell != NULL && pOurCell->isLSR()) || (pTheirCell != NULL && pTheirCell->isLSR()) )
   {
    for( int j = 0; j < m_followers.size(); j++ )
    {
     m_followers[j]->setToFollow(neighbors);
     leaders[iNewLeader]->addFollower(m_followers[j]);
    }
    
    m_followers.clear();
    
    setToFollow(neighbors);
    leaders[iNewLeader]->addFollower(this);
    return;
   }
   else
   {
    // follow path towards leader
    if( m_path.size() > 0 && curPos.dist(m_path.back().pos()) > getWidth() )
    {
     Cell* pTheirCell = leaders[iNewLeader]->getCell( m_path.back().pos() - leaders[iNewLeader]->getLSRPose().pos());
     if( pTheirCell != NULL && pTheirCell->isLSR() )
     {
      goTo( m_path.back() );
      return;
     }
    }
    
    goTo( leaderPaths[iNewLeader] );
    return;
   }
  }
 }
 
 if( gDebug > 5 ) cout<< "    " << getID() << " no leader to follow" << endl;
 
 /*
 // group with nearby wanderers into a wandering blob
 if( wanderers.size() > 0 )
 {
  for( int j = 0; j < m_followers.size(); j++ )
  {
   m_followers[j]->setToFollow();
   wanderers[0]->addFollower(m_followers[j]);
  }
  
  m_followers.clear();
  
  setToFollow();
  wanderers[0]->addFollower(this);
  return;
 }
 */
 Frontier frontier = m_LSR.frontier();
 
 if( m_path.size() > 0 )
 {
  if( curPos.dist( m_path.back().pos() ) > 1.5*getWidth() )
  {
   if( pickIndexInRange(0,20) != 0 )
   {
    // stick to current path ...
    return;
   }
  }
 }
 
 // Random walk to some point on current frontier
 if( frontier.size() > 0 )
 {
  Position bestRandPos = curPos;
  double bestRandPosValue = -1000.0;
  
  int iFrontierCells = 0;
  for( int i = 0; i < frontier.size(); i++ )
  {
   iFrontierCells += frontier[i].size();
  }
  
  for( int i = 0; i < 10; i++ )
  {
   int iRand = pickIndexInRange( 0, iFrontierCells );
   Position randPos = curPos;
   
   for( int j = 0; j < frontier.size(); j++ )
   {
    if( iRand < frontier[j].size() )
    {
     randPos += m_LSR.getCell(frontier[j][iRand])->point();
     break;
    }
    else
    {
     iRand -= frontier[j].size();
    }
   }
   
   double randPosValue = 10.0*(pickIndexInRange(0, 100))/100.0;
   
   double distDiv = 6.0 * m_pLaserProxy->GetMaxRange();
   double closestDistance = distDiv;
   for( int j = 0; j < allLeaders.size(); j++ )
   {
    closestDistance = std::min( closestDistance, randPos.dist( allLeaders[j]->getPosition() ) );
   }
   
   randPosValue += 5.0*(distDiv - closestDistance)/distDiv;
   
   randPosValue += 5.0*(M_PI - fabs((randPos - curPos).bearing().alDiff(m_pPosProxy->GetYaw())))/(M_PI);
   
   if( randPosValue > bestRandPosValue )
   {
    bestRandPos = randPos;
    bestRandPosValue = randPosValue;
   }
  }
  
  Position bestGoToPos = curPos;
  double bestGoToDist = curPos.dist( bestRandPos );
  
  for( int i = 0; i < m_LSR._lrr.size(); i++ )
  {
   Position testPos = curPos + m_LSR.getCell(m_LSR._lrr[i])->point();
   double dist = bestRandPos.dist( testPos );
   if( dist < bestGoToDist )
   {
    bestGoToDist = dist;
    bestGoToPos = testPos;
   }
  }
  
  if( gDebug > 8 )
  {
   drawRedX( bestRandPos, 0.05 );
   drawX( gColorGreen, bestGoToPos, 0.05 );
   usleep( 100000 );
  }
  
  m_path.clear();
  
  m_path.push_back( getPose() );
  Position relPos = bestGoToPos - curPos;
  m_path.push_back( Pose(bestGoToPos, relPos.bearing()) );
  
  goTo( m_path.back() );
 }
}

void PursuitAgent::clientRead()
{
 //while( m_pClient->Peek(0) )
 {
  m_pClient->ReadIfWaiting();
 }
}


void PursuitAgent::getNeighborFrontiers( AgentVector neighbors, Pose &curPose, vector<Frontier > &ourNeighborFrontiers, 
   vector<Frontier > &updatedNeighborFrontiers,
   vector<DubInt > &neighborObstacleEndpoints,
   vector<DubInt > &neighborObstacleBeginpoints )
{
 player_color_t color;
 
 // Process our previous frontier as if it is a neighbor frontier
 //
 // This is to handle situation where robot has self-expanded.
 if( m_prevFrontier.size() > 0 )
 {
  if( gDebug > 7 ) cout<< "    " << getID() << " has " << m_prevFrontier.size() << " prev frontier(s)" << endl;
  Pose neighborPose = m_prevLSR.center();
  Frontier ourNeighborFrontier;
  // Previous frontier for an agent expanding on its own acts like a neighbor's frontier
  for( int j = 0; j < m_prevFrontier.size(); j++ )
  {
   FrontierArc neighborArc = m_prevFrontier[j];
   FrontierArc translatedArc;
   Cell* pTranslatedCell;
   //Cell* pRotTranslatedCell;
   Position translatedPos;
   
   if( m_prevLSR.getCell(neighborArc[0])->isLabelA() )
   {
    Position arcCellPos = m_prevLSR.getCell(neighborArc[0])->point();
    Position globalPos = neighborPose.pos() + arcCellPos;
    translatedPos = neighborPose.pos() - curPose.pos() + arcCellPos;
    
    pTranslatedCell = m_LSR.getCell( translatedPos );
    
    if( pTranslatedCell != NULL )
    {
     neighborObstacleBeginpoints.push_back(pTranslatedCell->indexes());
    
     if( gDebug > 9 ) cout<< "    prev obstacle beginpoint at " << neighborObstacleBeginpoints.back().print() << endl;
    }
   }
   
   if( m_prevLSR.getCell(neighborArc[neighborArc.size()-1])->isLabelB() )
   {
    if( !(m_prevLSR.getCell(neighborArc[neighborArc.size()-1])->isLabelA()) )
    {
     Position arcCellPos = m_prevLSR.getCell(neighborArc[neighborArc.size()-1])->point();
     Position globalPos = neighborPose.pos() + arcCellPos;
     translatedPos = neighborPose.pos() - curPose.pos() + arcCellPos;
     
     pTranslatedCell = m_LSR.getCell( translatedPos );
     
     if( pTranslatedCell != NULL )
     {
      neighborObstacleEndpoints.push_back(pTranslatedCell->indexes());
      
      if( gDebug > 9 ) cout<< "    prev obstacle endpoint at " << neighborObstacleEndpoints.back().print() << endl;
     }
    }
   }
   
   //if( gDebug > 7 ) cout<< "      " << neighbors[i]->getID() << " had arc of size " << neighborArc.size() << endl;
   
   for( int k = 0; k < neighborArc.size(); k++ )
   {
    Position arcCellPos = m_prevLSR.getCell(neighborArc[k])->point();
    Position globalPos = neighborPose.pos() + arcCellPos;
    translatedPos = neighborPose.pos() - curPose.pos() + arcCellPos;
    
    pTranslatedCell = m_LSR.getCell( translatedPos );
    
    //cout<< "Arc cell " << k << " at " << arcCellPos.print() << " relative to " << neighborPos.print() << endl;
    //cout<< "  at " << globalPos.print() << " in global frame" << endl;
    
    //cout<< "  at " << translatedPos.print() << " relative to " << curPos.print() << endl;
    
    if( pTranslatedCell == NULL )
    {
     // Means that position is outside of our LSR grid
     if( translatedArc.size() >= 0 )
     {
      // Put relevant fragment into our stored arcs
      ourNeighborFrontier.push_back(translatedArc);
      translatedArc = FrontierArc();
     }
     
     color.alpha = 0;
     color.red = 255;
     color.green = 0;
     color.blue = 255;
    }
    else
    {
     // If cell is outside of our LSR or on our frontier, leave it in updated neighbor's frontier
     if( (!(pTranslatedCell->isLSR()) || pTranslatedCell->isFrontier()) && !(pTranslatedCell->isObstacle()) )
     {
      //updatedNeighborArc.push_back(neighborArc[k]);
      color.alpha = 0;
      color.red = 0;
      color.green = 0;
      color.blue = 255;
     }
     else
     {
      color.alpha = 0;
      color.red = 255;
      color.green = 0;
      color.blue = 0;
     }
     
     translatedArc.push_back( pTranslatedCell->indexes() );
    }
    
    //if( bDebugDisplay ) drawSquare( color, globalPos, 0.01, true );
   }
   
   if( translatedArc.size() >= 0 )
   {
    ourNeighborFrontier.push_back(translatedArc);
   }
  }
  
  if( ourNeighborFrontier.size() > 0 )
  {
   ourNeighborFrontiers.push_back(ourNeighborFrontier);
  }
 }
 
 
 // Get and process arcs from all neighbors
 for( int i = 0; i < neighbors.size(); i++ ){
  updatedNeighborFrontiers.push_back( Frontier() );
  
  if( neighbors[i]->isLeader() && (neighbors[i]->getID() != getID()) )
  {
   Pose neighborPose = neighbors[i]->getLSRPose();
   
   if( neighborPose.pos().dist(curPose.pos()) > gOverlapFactor*(getSensorRange() + neighbors[i]->getSensorRange()) )
   {
    if( gDebug > 7 ) cout<< "    " << getID() << " ignoring distant agent " << neighbors[i]->getID() << endl;
    continue;
   }
   
   if( gDebug > 7 ) cout<< "    " << getID() << " updating agent " << neighbors[i]->getID() << " frontiers" << endl;
   
   // In asynchronous implementation, would need to take over control of
   // neighbors frontiers, so they don't send it to anyone else
   Frontier neighborFrontier = neighbors[i]->getFrontier();
   
   Frontier updatedNeighborFrontier;
   Frontier ourNeighborFrontier;
   
   for( int j = 0; j < neighborFrontier.size(); j++ )
   {
    FrontierArc neighborArc = neighborFrontier[j];
    FrontierArc updatedNeighborArc;
    
    // Represent neighbors arc in our grid, requires either grids to
    // (mostly) be aligned or to have some smart handling of correspondence.
    // In experimental 
    FrontierArc translatedArc;
    Cell* pTranslatedCell;
    //Cell* pRotTranslatedCell;
    Position translatedPos;
    
    if( neighbors[i]->getCell(neighborArc[0])->isLabelA() )
    {
     Position arcCellPos = neighbors[i]->getCellPosition(neighborArc[0]);
     Position globalPos = neighborPose.pos() + arcCellPos;
     translatedPos = neighborPose.pos() - curPose.pos() + arcCellPos;
     
     pTranslatedCell = m_LSR.getCell( translatedPos );
     
     if( pTranslatedCell != NULL )
     {
      neighborObstacleBeginpoints.push_back(pTranslatedCell->indexes());
     
      if( gDebug > 9 ) cout<< "    neighbor obstacle beginpoint at " << neighborObstacleBeginpoints.back().print() << endl;
     }
    }
    
    if( neighbors[i]->getCell(neighborArc[neighborArc.size()-1])->isLabelB() )
    {
     if( !(neighbors[i]->getCell(neighborArc[neighborArc.size()-1])->isLabelA()) )
     {
      Position arcCellPos = neighbors[i]->getCellPosition(neighborArc[neighborArc.size()-1]);
      Position globalPos = neighborPose.pos() + arcCellPos;
      translatedPos = neighborPose.pos() - curPose.pos() + arcCellPos;
      
      pTranslatedCell = m_LSR.getCell( translatedPos );
      
      if( pTranslatedCell != NULL )
      {
       neighborObstacleEndpoints.push_back(pTranslatedCell->indexes());
       
       if( gDebug > 9 ) cout<< "    neighbor obstacle endpoint at " << neighborObstacleEndpoints.back().print() << endl;
      }
     }
    }
    
    if( gDebug > 7 ) cout<< "      " << neighbors[i]->getID() << " had arc of size " << neighborArc.size() << endl;
    
    bool bPrevErased = false;
    for( int k = 0; k < neighborArc.size(); k++ )
    {
     Position arcCellPos = neighbors[i]->getCellPosition(neighborArc[k]);
     Position globalPos = neighborPose.pos() + arcCellPos;
     translatedPos = neighborPose.pos() - curPose.pos() + arcCellPos;
     
     pTranslatedCell = m_LSR.getCell( translatedPos );
     
     //cout<< "Arc cell " << k << " at " << arcCellPos.print() << " relative to " << neighborPos.print() << endl;
     //cout<< "  at " << globalPos.print() << " in global frame" << endl;
     
     //cout<< "  at " << translatedPos.print() << " relative to " << curPos.print() << endl;
     
     if( pTranslatedCell == NULL )
     {
      // Means that position is outside of our LSR grid
      
      // Neighbor should keep this cell
      updatedNeighborArc.push_back(neighborArc[k]);
      
      if( translatedArc.size() > 0 )
      {
       // Put any relevant fragment we were accumulating into our stored arcs
       ourNeighborFrontier.push_back(translatedArc);
       translatedArc = FrontierArc();
      }
      
      color.alpha = 0;
      color.red = 255;
      color.green = 0;
      color.blue = 255;
     }
     else
     {
      bool bErase = false;
      
      if( pTranslatedCell->isObstacle() )
      {
       bErase = true;
      }
      else if( pTranslatedCell->isFrontier() )
      {
       bErase = true;
      }
      else if( pTranslatedCell->isLSR() )
      {
       bErase = true;
      }
      else
      {
       // Outside visibility, probably don't erase
       
       bErase = false;
       
       vector<Cell* > surroundingCells = pTranslatedCell->neighbours();
       
       int iObstacle = 0;
       for( int l = 0; l < surroundingCells.size(); l++ )
       {
        if( surroundingCells[l]->isObstacle() )
        {
         // This catches certain circumstances where one cell of neighbor's arc lies inside
         // an obstacle in this agent's view.  Happens more when grids are poorly aligned.
         // May not be necessary or best way to catch these conditions
         iObstacle++;
         
         if( iObstacle > 2 )
         {
          bErase = true;
         }
        }
        else if( surroundingCells[l]->isLSR() )
        {
         bErase = false;
         break;
        }
       }
       
       if( iObstacle > 0 ) cout<< iObstacle << " out of " << surroundingCells.size() << endl;
      }
      
      if( bErase && !bPrevErased && k == neighborArc.size()-1 )
      {
       if( neighbors[i]->getCell(neighborArc[k])->isLabelB() )
       {
        // Save obstacle end point
        bErase = false;
       }
      }
      
      // If cell is outside of our LSR, leave it in updated neighbor's frontier
      if( !bErase )
      {
       if( k == 1 && bPrevErased ) 
       {
        if( neighbors[i]->getCell(neighborArc[0])->isLabelA() )
        {
         // Save obstacle begin point
         updatedNeighborArc.push_back(neighborArc[0]);
        }
       }
       updatedNeighborArc.push_back(neighborArc[k]);
       
       color.alpha = 0;
       color.red = 0;
       color.green = 0;
       color.blue = 255;
      }
      else
      {
       if( updatedNeighborArc.size() > 0 )
       {
        if( updatedNeighborArc.size() >= gMinMergeArcSize )
        {
         if( gDebug > 7 ) cout<< "      adding neighbor arc of size " << (updatedNeighborArc.size()) << endl;
         updatedNeighborFrontier.push_back(updatedNeighborArc);
        }
        else
        {
         if( gDebug > 7 ) cout<< "      clearing temp neighbor arc of size " << (updatedNeighborArc.size()) << endl;
         updatedNeighborFrontier.push_back(updatedNeighborArc);
        }
        
        updatedNeighborArc = FrontierArc();
       }
       
       color.alpha = 0;
       color.red = 255;
       color.green = 0;
       color.blue = 0;
      }
      
      // Add cell to our translated arc
      translatedArc.push_back( pTranslatedCell->indexes() );
      
      bPrevErased = bErase;
     }
     
     //if( bDebugDisplay ) drawSquare( color, globalPos, 0.01, true );
     
    }
    
    if( updatedNeighborArc.size() >= gMinMergeArcSize )
    {
     if( gDebug > 7 ) cout<< "      replacing neighbor arc with one of size " << updatedNeighborArc.size() << endl;
     updatedNeighborFrontier.push_back(updatedNeighborArc);
    }
    else
    {
     if( gDebug > 7 ) cout<< "      clearing temp neighbor arc (end) of size " << (updatedNeighborArc.size()) << endl;
    }
    
    if( translatedArc.size() > 0 )
    {
     ourNeighborFrontier.push_back(translatedArc);
    }
   }
   
   updatedNeighborFrontiers[i] = updatedNeighborFrontier;
   
   if( ourNeighborFrontier.size() > 0 )
   {
    ourNeighborFrontiers.push_back(ourNeighborFrontier);
   }
   
  }
 }
 
}


void PursuitAgent::classifyOurFrontier( AgentVector neighbors, Pose &curPose, vector<Frontier > &ourNeighborFrontiers, 
   vector<DubInt > &neighborObstacleEndpoints,
   vector<DubInt > &neighborObstacleBeginpoints,
   bool bDebugDisplay )
{
 player_color_t color;
 
 // Classify our frontier arcs based on intersections with neighbors' arcs
 Frontier orangeFrontier;
 Frontier purpleFrontier;
 FrontierArc tempFrontierArc;
 FrontierArc tempFreeArc;
 bool bOrange = true;
 KeepColor keepColor = KEEP_UNKNOWN;
 
 LSRBound lsrBound = m_LSR.lsrBound();
 
 // Need to find where on lsrBound to start classification loop, if there is an obstacle then we'll start there
 // so that we don't split a frontier arc.
 int iOffset = 0;
 for( int l = 0; l < lsrBound.size(); l++ )
 {
  int iIndex = (l + iOffset)%lsrBound.size();
  DubInt iBoundCell = lsrBound[iIndex];
  Cell* pBoundCell = m_LSR.getCell(iBoundCell);
  
  if( pBoundCell->isObstacle() &&  
    m_LSR.getCell(lsrBound[(l + 1 + iOffset)%lsrBound.size()])->isObstacle() &&
    m_LSR.getCell(lsrBound[(l - 1 + iOffset)%lsrBound.size()])->isObstacle() )
  {
   iOffset = l;
   break;
  }
 }
 
 // Loop classifying boundary of our LSR.  At this point, m_frontier has been initialized to
 // be all the free arcs of LSR (or what frontier would be if there were no neighbors).  
 // After this loop we'll set m_frontier to its proper value after classifing free arcs.
 int iCurArc = -1;
 for( int l = 0; l < lsrBound.size(); l++ )
 {
  int iIndex = (l + iOffset)%lsrBound.size();
  DubInt iBoundCell = lsrBound[iIndex];
  Cell* pBoundCell = m_LSR.getCell(iBoundCell);
  Position globalPos = curPose.pos() + pBoundCell->point();
  int iThisArc = -1;
  
  // Is this LSR boundary cell on the free part of boundary?  (ie, it's a potential frontier cell)
  bool bFrontierCell = false;
  for( int i = 0; (i < m_frontier.size() && !bFrontierCell); i++ )
  {
   FrontierArc arc = m_frontier[i];
   for( int j = 0; (j < arc.size() && !bFrontierCell); j++ )
   {
    if( iBoundCell == arc[j] )
    {
     bFrontierCell = true;
     iThisArc = i;
     break;
    }
   }
  }
  
  // Controls whether to break arcs into radial/curved segments or connect them
  bool bBreakArc = false;
  //bool bBreakArc = (iCurArc >= 0);
  //bBreakArc &= (iCurArc != iThisArc);
  
  // Build list of our free arcs (result should be the similar to m_LSR.frontier())
  if( !bFrontierCell || (bBreakArc) )
  {
   if( tempFreeArc.size() > 0 )
   {
    m_freeArcs.push_back(tempFreeArc);
    tempFreeArc = FrontierArc();
   }
  }
  
  if( bFrontierCell )
  {
   if( tempFreeArc.size() > 0 && tempFreeArc[tempFreeArc.size()-1] == iBoundCell )
   {
    cerr<< "***Warning***:  Free arc: Repeated cell in boundary at " << iBoundCell.print() << ".  Ignoring repeat." << endl;
    cerr<< "                Cell num " << iIndex << " out of " << lsrBound.size() << endl;
   }
   else
   {
    tempFreeArc.push_back(iBoundCell);
   }
  }
  
  if( !bFrontierCell || bBreakArc )
  {
   if( tempFrontierArc.size() > 0 )
   {
    if( tempFrontierArc.size() >= gMinMergeArcSize )
    {
     if( bOrange )
     {
      if( gDebug > 8 ) cout<< "Pushing temp arc with " << tempFrontierArc.size() << " cells to orange" << endl;
      orangeFrontier.push_back(tempFrontierArc);
     }
     else
     {
      if( gDebug > 8 ) cout<< "Pushing temp arc with " << tempFrontierArc.size() << " cells to purple" << endl;
      purpleFrontier.push_back(tempFrontierArc);
     }
    }
    
    if( gDebug > 8 ) cout<< "Clearing temp arc with " << tempFrontierArc.size() << " cells" << endl;
    tempFrontierArc = FrontierArc();
   }
  }
  
  // Build lists of free arc segments classified by color, one color will be our true frontier
  if( bFrontierCell )
  {
   //if( gDebug > 8 ) cout<< "  Adding cell at " << globalPos.print() << " to temp frontier arc" << endl;
   if( tempFrontierArc.size() > 0 && tempFrontierArc[tempFrontierArc.size()-1] == iBoundCell )
   {
    cerr<< "***Warning***:  Frontier Arc: Repeated cell in boundary at " << iBoundCell.print() << ".  Ignoring repeat." << endl;
    cerr<< "                Cell num " << iIndex << " out of " << lsrBound.size() << endl;
   }
   else
   {
    //if( gDebug > 9 ) cout<< "      Adding frontier cell " << iBoundCell.print() << endl;
    tempFrontierArc.push_back(iBoundCell);
   }
  }
  
  if( iThisArc >= 0 )
  {
   iCurArc = iThisArc;
  }
  
  if( bDebugDisplay )
  {
   if( bOrange )
   {
    color.alpha = 0;
    color.red = 255;
    color.green = 255;
    color.blue = 0;
    
    if( pBoundCell->isFrontier() )
    {
     color.green = 125;
    }
   }
   else
   {
    color.alpha = 0;
    color.red = 255;
    color.green = 0;
    color.blue = 255;
    
    if( pBoundCell->isFrontier() )
    {
     color.red = 150;
    }
   }
   
   drawSquare( color, globalPos, 0.01, true );
  }
  
  //if( gDebug > 8 && pBoundCell->isFrontier() ) cout<< "Checking frontier cell " << iBoundCell.print() << endl;
  
  // Find any intersections and, if there's an intersection, consider switching colors
  for( int i = 0; i < ourNeighborFrontiers.size(); i++ )
  {
   for( int j = 0; j < ourNeighborFrontiers[i].size(); j++ )
   {
    FrontierArc ourNeighborArc = ourNeighborFrontiers[i][j];
    
    for( int k = 0; k < ourNeighborArc.size(); k++ )
    {
     bool bChangeIntToExt = false;
     bool bChangeExtToInt = false;
     
     bool bObsEndpoint = false;
     bool bObsBeginpoint = false;
     
     if( (k == 0) )
     {
      for( int l = 0; l < neighborObstacleBeginpoints.size(); l++ )
      {
       if( ourNeighborArc[k] == neighborObstacleBeginpoints[l] )
       {
        bObsBeginpoint = true;
        break;
       }
      }
     }
     
     if( (k == (ourNeighborArc.size() - 1)) )
     {
      for( int l = 0; l < neighborObstacleEndpoints.size(); l++ )
      {
       if( ourNeighborArc[k] == neighborObstacleEndpoints[l] )
       {
        bObsEndpoint = true;
        break;
       }
      }
     }
     
     int iPrevLSRFlag = 0;
     int iNextLSRFlag = 0;
     
     bool bObsPoint = (bObsBeginpoint && !bObsEndpoint) || (!bObsBeginpoint && bObsEndpoint);
     
     if(bObsPoint || iBoundCell == ourNeighborArc[k])
     {
      // LSRFlag values:
      // -1 : outside LSR
      //  0 : on boundary
      // +1 : inside LSR
      
      // have to check up to min arc size cells to classify as a crossing
      if( k >= 1 )
      {
       Cell* pPrevArcCell = m_LSR.getCell(ourNeighborArc[k-1]);
       if( !(pPrevArcCell->isBoundary()) )
       {
        if( pPrevArcCell->isLSR() )
        {
         iPrevLSRFlag = 1;
         for( int m = 2; m <= k && m <= gMinMergeArcSize; m++ )
         {
          pPrevArcCell = m_LSR.getCell(ourNeighborArc[k-m]);
          if( (pPrevArcCell->isBoundary()) || !(pPrevArcCell->isLSR()) )
          {
           iPrevLSRFlag = 0;
           break;
          }
         }
        }
        else
        {
         iPrevLSRFlag = -1;
         for( int m = 2; m <= k && m <= gMinMergeArcSize; m++ )
         {
          pPrevArcCell = m_LSR.getCell(ourNeighborArc[k-m]);
          if( (pPrevArcCell->isBoundary()) || (pPrevArcCell->isLSR()) )
          {
           iPrevLSRFlag = 0;
           break;
          }
         }
        }
       }
      }
      
      if( k <= (ourNeighborArc.size() - 2) )
      {
       Cell* pNextArcCell = m_LSR.getCell(ourNeighborArc[k+1]);
       if( !(pNextArcCell->isBoundary()) )
       {
        if( pNextArcCell->isLSR() )
        {
         iNextLSRFlag = 1;
         for( int m = 2; m <= (ourNeighborArc.size() - 1 - k) && m <= gMinMergeArcSize; m++ )
         {
          pNextArcCell = m_LSR.getCell(ourNeighborArc[k+m]);
          if( (pNextArcCell->isBoundary()) || !(pNextArcCell->isLSR()) )
          {
           iNextLSRFlag = 0;
           break;
          }
         }
        }
        else
        {
         iNextLSRFlag = -1;
         for( int m = 2; m <= (ourNeighborArc.size() - 1 - k) && m <= gMinMergeArcSize; m++ )
         {
          pNextArcCell = m_LSR.getCell(ourNeighborArc[k+m]);
          if( (pNextArcCell->isBoundary()) || (pNextArcCell->isLSR()) )
          {
           iNextLSRFlag = 0;
           break;
          }
         }
        }
       }
      }
     }
     
     // Cells are the same
     if(iBoundCell == ourNeighborArc[k])
     {
      if( bObsPoint && bObsBeginpoint && iNextLSRFlag != -1)
      {
       pBoundCell->setIsLabelA();
       bChangeIntToExt = true;
       if( gDebug > 1 ) cout<< "  Int to Ext, based on match with obs begin point" << endl;
      }
      else if( bObsPoint && bObsEndpoint && iPrevLSRFlag != -1)
      {
       pBoundCell->setIsLabelB();
       bChangeExtToInt = true;
       if( gDebug > 1) cout<< "  Ext to Int, based on match with obst end point" << endl;
      }
      else if( !bObsPoint )
      {
       // Intersect-crossing
       if( iPrevLSRFlag == -1 && iNextLSRFlag == 1 )
       {
        bChangeIntToExt = true;
        if( gDebug > 1 ) cout<< "  Int to Ext, based on intersect crossing 1" << endl;
       }
       else if( iPrevLSRFlag == 1 && iNextLSRFlag == -1 )
       {
        bChangeExtToInt = true;
        if( gDebug > 1 ) cout<< "  Ext to Int, based on intersect crossing 2" << endl;
       }
       // Intersect-tangent
       else if( iPrevLSRFlag == 1 && iNextLSRFlag == 1 )
       {
        // No change
       }
       else if( iPrevLSRFlag == -1 && iNextLSRFlag == -1 )
       {
        // No change
       }
       // Intersect-join
       else if( iPrevLSRFlag == 1 && iNextLSRFlag == 0 )
       {
        // No change
       }
       else if( iPrevLSRFlag == -1 && iNextLSRFlag == 0 )
       {
        bChangeIntToExt = true;
        if( gDebug > 1 ) cout<< "  Int to Ext, based on intersect join 2" << endl;
       }
       else if( iPrevLSRFlag == 0 && iNextLSRFlag == 1 )
       {
        // No change
       }
       else if( iPrevLSRFlag == 0 && iNextLSRFlag == -1 )
       {
        bChangeExtToInt = true;
        if( gDebug > 1 ) cout<< "  Ext to Int, based on intersect join 4" << endl;
       }
      }
     }
     else
     {
      // Check it neighbor arc dead ends near obstacle boundary
      vector<Cell* > cellNeighbors = m_LSR.getCell(iBoundCell)->neighbours();
      
      for( int n = 0; n < cellNeighbors.size(); n++ )
      {
       // Adjacent cell
       if(cellNeighbors[n]->indexes() == ourNeighborArc[k])
       {
        if( bObsPoint && bObsBeginpoint && (iNextLSRFlag == 1 || (cellNeighbors[n]->isLSR() && !cellNeighbors[n]->isBoundary())) )
        {
         bChangeIntToExt = true;
         if( gDebug > 1 ) cout<< "  Int to Ext, based on first arc cell next to obstacle" << endl;
         break;
        }
        else if( bObsPoint && bObsEndpoint && (iPrevLSRFlag == 1 || (cellNeighbors[n]->isLSR() && !cellNeighbors[n]->isBoundary())) )
        {
         bChangeExtToInt = true;
         if( gDebug > 1) cout<< "  Ext to Int, based on last arc cell next to obstacle" << endl;
         break;
        }
       }
      }
      
      if( !(bChangeExtToInt || bChangeIntToExt) )
      {
       // Cross with no intersecting cells
       bool bCross = false;
       if( k < (ourNeighborArc.size() - 1) )
       {
        int iNextIndex = (l + iOffset + 1)%lsrBound.size();
        DubInt iNextBoundCell = lsrBound[iNextIndex];
        
        if( iNextBoundCell.i() != iBoundCell.i() && iNextBoundCell.j() != iBoundCell.j() )
        {
         if( iBoundCell.i() == ourNeighborArc[k].i() && iNextBoundCell.j() == ourNeighborArc[k].j() )
         {
          if( iBoundCell.j() == ourNeighborArc[k+1].j() && iNextBoundCell.i() == ourNeighborArc[k+1].i() )
          {
           bChangeExtToInt = true;
           if( gDebug > 1 ) cout<< "  Ext to Int, based on crossing" << endl;
          }
         }
         else if( iBoundCell.i() == ourNeighborArc[k+1].i() && iNextBoundCell.j() == ourNeighborArc[k+1].j() )
         {
          if( iBoundCell.j() == ourNeighborArc[k].j() && iNextBoundCell.i() == ourNeighborArc[k].i() )
          {
           bChangeIntToExt = true;
           if( gDebug > 1 ) cout<< "  Int to Ext, based on crossing" << endl;
          }
         }
        }
       }
      }
     }
     
     if(bChangeIntToExt || bChangeExtToInt)
     {
      if( gDebug > 1 ) cout<< "  Transition at cell " << k << " at " << globalPos.print() << endl;
      
      bool bChange = false;
      
      if( bChangeIntToExt )
      {
       // Moving from internal to external
       if( keepColor == KEEP_UNKNOWN )
       {
        keepColor = (bOrange ? KEEP_PURPLE : KEEP_ORANGE);
        if( gDebug > 1 ) cout<< "  Setting keep color to " << (bOrange ? "KEEP_PURPLE" : "KEEP_ORANGE") << endl;
        bOrange = !bOrange;
        
        if( gDebug > 1 ) cout<< "  Switched color to " << (bOrange ? "ORANGE" : "PURPLE") << endl;
        bChange = true;
       }
       else
       {
        // Were we already external?
        if( keepColor == (bOrange ? KEEP_ORANGE : KEEP_PURPLE ) )
        {
         // Do nothing
         if( gDebug > 1 ) cout<< "  Maintaining current color" << endl;
        }
        else
        {
         bOrange = !bOrange;
         if( gDebug > 1 ) cout<< "  Switched color to " << (bOrange ? "ORANGE" : "PURPLE") << endl;
         bChange = true;
        }
       }
       
      }
      else if( bChangeExtToInt )
      {
       // Moving from external to internal
       if( keepColor == KEEP_UNKNOWN )
       {
        keepColor = (bOrange ? KEEP_ORANGE : KEEP_PURPLE);
        
        if( gDebug > 1 ) cout<< "  Setting keep color to " << (bOrange ? "KEEP_ORANGE" : "KEEP_PURPLE") << endl;
        bOrange = !bOrange;
        
        if( gDebug > 1 ) cout<< "  Switched color to " << (bOrange ? "ORANGE" : "PURPLE") << endl;
        bChange = true;
       }
       else
       {
        // Were we already internal?
        if( keepColor == (bOrange ? KEEP_PURPLE : KEEP_ORANGE ) )
        {
         // Do nothing
         if( gDebug > 1 ) cout<< "  Maintaining current color" << endl;
        }
        else
        {
         bOrange = !bOrange;
         if( gDebug > 1 ) cout<< "  Switched color to " << (bOrange ? "ORANGE" : "PURPLE") << endl;
         bChange = true;
        }
       }
      }
      
      if( bChange )
      {
       if( tempFrontierArc.size() > 0 )
       {
        if( tempFrontierArc.size() >= gMinMergeArcSize )
        {
         // Color just switched, push to previous color
         if( !(bOrange) )
         {
          if( gDebug > 8 ) cout<< "Pushing temp arc with " << tempFrontierArc.size() << " cells to orange (color)" << endl;
          orangeFrontier.push_back(tempFrontierArc);
         }
         else
         {
          if( gDebug > 8 ) cout<< "Pushing temp arc with " << tempFrontierArc.size() << " cells to purple (color)" << endl;
          purpleFrontier.push_back(tempFrontierArc);
         }
        }
        
        if( gDebug > 8 ) cout<< "Clearing temp arc (color)" << endl;
        tempFrontierArc = FrontierArc();
        
        if( bFrontierCell )
        {
         tempFrontierArc.push_back(iBoundCell);
        }
       }
      }
     }
    }
   }
  }
  
  
  /*
  cout<< "Waiting for go ahead..." << endl;
  char input[10];
  cin>> input;
  */
 }
 
 if( tempFreeArc.size() > 0 )
 {
  if( m_freeArcs.size() > 0 && m_freeArcs[0][0] == lsrBound[iOffset] )
  {
   m_freeArcs[0].insert( m_freeArcs[0].begin(), tempFreeArc.begin(), tempFreeArc.end() );
  }
  else
  {
   m_freeArcs.push_back(tempFreeArc);
  }
 }
 
 if( tempFrontierArc.size() >= gMinMergeArcSize )
 {
  if( bOrange )
  {
   if( orangeFrontier.size() > 0 && orangeFrontier[0][0] == lsrBound[iOffset] )
   {
    if( gDebug > 8 ) cout<< "  Merging temp arc with " << tempFrontierArc.size() << " cells to orange" << endl;
    orangeFrontier[0].insert( orangeFrontier[0].begin(), tempFrontierArc.begin(), tempFrontierArc.end() );
   }
   else
   {
    if( gDebug > 8 ) cout<< "  Pushing temp arc with " << tempFrontierArc.size() << " cells to orange" << endl;
    orangeFrontier.push_back(tempFrontierArc);
   }
  }
  else
  {
   if( purpleFrontier.size() > 0 && purpleFrontier[0][0] == lsrBound[iOffset] )
   {
    if( gDebug > 8 ) cout<< "  Merging temp arc with " << tempFrontierArc.size() << " cells to purple" << endl;
    purpleFrontier[0].insert( purpleFrontier[0].begin(), tempFrontierArc.begin(), tempFrontierArc.end() );
   }
   else
   {
    if( gDebug > 8 ) cout<< "  Pushing temp arc with " << tempFrontierArc.size() << " cells to purple" << endl;
    purpleFrontier.push_back(tempFrontierArc);
   }
  }
 }
 
 // Keep one of the two colors as our new frontier
 if( keepColor == KEEP_ORANGE || keepColor == KEEP_UNKNOWN )
 {
  if( gDebug > 1 ) cout<< "Keeping orange frontier, with " << orangeFrontier.size() << " arcs"<< endl;
  m_frontier = orangeFrontier;
 }
 else
 {
  if( gDebug > 1 ) cout<< "Keeping purple frontier, with " << purpleFrontier.size() << " arcs"<< endl;
  m_frontier = purpleFrontier;
 }
 
 if( gDebug > 8 )
 {
  cout<< "  Updated frontier size:" << endl;
  cout<< "    " << m_frontier.size() << " frontier arcs" << endl;
  for( int i = 0; i < m_frontier.size(); i++ )
  {
   cout<< "     Arc " << i << " has " << m_frontier[i].size() << " cells" << endl;
  }
 }
}


void PursuitAgent::updateNeighborFrontiers( AgentVector neighbors, Pose &curPose, vector<Frontier > &updatedNeighborFrontiers )
{
 player_color_t color;
 
 // Finish processing neighbor frontiers and send them back to neighbors
 for( int i = 0; i < neighbors.size(); i++ )
 {
  if( neighbors[i]->isLeader() && (neighbors[i]->getID() != getID()) )
  {
   Position neighborPos = neighbors[i]->getLSRPose().pos();
   
   if( neighborPos.dist(curPose.pos()) > gOverlapFactor*(getSensorRange() + neighbors[i]->getSensorRange()) )
   {
    if( gDebug > 7 ) cout<< "    " << getID() << " ignoring distant agent " << neighbors[i]->getID() << endl;
    continue;
   }
   
   if( gDebug > 7 ) cout<< "    " << getID() << " setting agent " << neighbors[i]->getID() << " frontiers" << endl;
   
   
   Frontier newFrontier;
   for( int j = 0; j < updatedNeighborFrontiers[i].size(); j++ )
   {
    
    bool bErase = false;
    FrontierArc updatedNeighborArc = updatedNeighborFrontiers[i][j];
    
    if( updatedNeighborArc.size() <= 3 )
    {
     if( neighbors[i]->getCell(updatedNeighborArc[0])->isLabelA() )
     {
      if( neighbors[i]->getCell(updatedNeighborArc[updatedNeighborArc.size()-1])->isLabelB() )
      {
       bErase = true;
      }
     }
    }
    /*
    if( updatedNeighborArc.size() < gMinArcSize )
    {
     // Under some circumstances we want to remove small neighbor arcs because
     // they're just the result of poor grid alignment.  However, under other circumstances
     // they're important to keep, so this is a tough question.
     
     if( neighbors[i]->getCell(updatedNeighborArc[0])->isLabelA() )
     {
      continue;
     }
     
     if( neighbors[i]->getCell(updatedNeighborArc[updatedNeighborArc.size()-1])->isLabelB() )
     {
      continue;
     }
     
     FrontierArc translatedArc;
     Cell* pTranslatedCell;
     //Cell* pRotTranslatedCell;
     Position translatedPos;
     
     Position arcCellPos = neighbors[i]->getCellPosition(updatedNeighborArc[0]);
     Position globalPos = arcCellPos + neighborPos;
     translatedPos = arcCellPos + neighborPos - curPose.pos();
     
     pTranslatedCell = m_LSR.getCell( translatedPos );
     
     if( pTranslatedCell != NULL )
     {
      vector<Cell* > nCells = pTranslatedCell->neighbours();
      
      for( int m = 0; m < nCells.size(); m++ )
      {
       if( !(nCells[m]->isUnknown()) )
       {
        bErase = true;
        break;
       }
      }
      
      for( int k = 0; (bErase && k < m_frontier.size()); k++ )
      {
       bool bKill = false;
       int iLSRCount = 0;
       FrontierArc ourArc = m_frontier[k];
       
       for( int l = 0; (bErase && l < ourArc.size()); l++ )
       {
        for( int m = 0; m < nCells.size(); m++ )
        {
         if( nCells[m]->indexes() == ourArc[l] )
         {
          bErase = false;
          break;
         }
        }
       }
      }
     }
    }*/
    
    // This is to catch errors where we erase a neighbors obstacle end point, but
    // because of quantization our arc doesn't quite hit the obstacle theirs did
    
    Pose neighborPose = neighbors[i]->getLSRPose();
    Cell* pCell1 = neighbors[i]->getCell(updatedNeighborArc[0]);
    Cell* pCell2 = neighbors[i]->getCell(updatedNeighborArc[updatedNeighborArc.size()-1]);
   
    if( !pCell1->isLabelA() && pCell1->isFrontier() )
    {
     // Did we perhaps erase continuation of this arc because of obstacles
     // which neighbor might not be able to see?
     bool bSet = false;
     Position translatedPos = neighborPose.pos() - curPose.pos() + pCell1->point();
     Cell* pTranslatedCell = m_LSR.getCell( translatedPos );
     
     if( pTranslatedCell != NULL )
     {
      for( int x = -1; x <= 1; x++ )
      {
       for( int y = -1; y <= 1; y++ )
       {
        if( x != 0 || y != 0 )
        {
         DubInt iTheirs = pCell1->indexes() + DubInt(x,y);
         Cell* pTheirCell = neighbors[i]->getCell( iTheirs );
         DubInt iOurs = pTranslatedCell->indexes() + DubInt(x,y);
         Cell* pOurCell = m_LSR.getCell( iOurs );
         
         if( pTheirCell != NULL && pOurCell != NULL )
         {
          if( pTheirCell->isFrontier() && pOurCell->isObstacle() )
          {
           bSet = true;
          }
         }
        }
       }
      }
     
      vector<Cell* > nCells = pTranslatedCell->neighbours();
     
      for( int l = 0; l < nCells.size(); l++ )
      {
       if( (m_LSR.getCell(nCells[l]->indexes())->isLabelA()) )
       {
        bSet = false;
       }
       
       for( int a = 0; a < m_frontier.size(); a++ )
       {
        FrontierArc ourArc = m_frontier[a];
        
        if( ourArc[0] == nCells[l]->indexes() )
        {
         bSet = false;
        }
        
        if( ourArc[ourArc.size()-1] == nCells[l]->indexes() )
        {
         bSet = false;
        }
       }
      }
    
      if( bSet )
      {
       if( gDebug > 7 ) cout<< getID() << " setting error correction neighbor obstacle begin point for neighbor's frontier" << endl;
       pCell1->setIsLabelA();
       if( gDebug >= 9 ) drawSquare( gColorGreen, neighborPose.pos() + pCell1->point(), 0.05, false );
      }
     }
    }
    
    if( !pCell2->isLabelB() && pCell2->isFrontier() )
    {
     // Did we perhaps erase continuation of this arc because of obstacles
     // which neighbor might not be able to see?
     bool bSet = false;
     Position translatedPos = neighborPose.pos() - curPose.pos() + pCell2->point();
     Cell* pTranslatedCell = m_LSR.getCell( translatedPos );
     
     if( pTranslatedCell != NULL )
     {
      for( int x = -1; x <= 1; x++ )
      {
       for( int y = -1; y <= 1; y++ )
       {
        if( x != 0 || y != 0 )
        {
         Cell* pTheirCell = neighbors[i]->getCell( pCell2->indexes() + DubInt(x,y) );
         Cell* pOurCell = m_LSR.getCell( pTranslatedCell->indexes() + DubInt(x,y) );
         
         if( pTheirCell != NULL && pOurCell != NULL )
         {
          if( pTheirCell->isFrontier() && pOurCell->isObstacle() )
          {
           bSet = true;
          }
         }
        }
       }
      }
     
      vector<Cell* > nCells = pTranslatedCell->neighbours();
     
      for( int l = 0; l < nCells.size(); l++ )
      {
       if( (m_LSR.getCell(nCells[l]->indexes())->isLabelB()) )
       {
        bSet = false;
       }
       
       for( int a = 0; a < m_frontier.size(); a++ )
       {
        FrontierArc ourArc = m_frontier[a];
        
        if( ourArc[0] == nCells[l]->indexes() )
        {
         bSet = false;
        }
        
        if( ourArc[ourArc.size()-1] == nCells[l]->indexes() )
        {
         bSet = false;
        }
       }
      }
     
      if( bSet )
      {
       if( gDebug > 7 ) cout<< getID() << " setting error correction neighbor obstacle end point for neighbor's frontier" << endl;
       pCell2->setIsLabelB();
       if( gDebug >= 9 ) drawSquare( gColorRed, neighborPose.pos() + pCell2->point(), 0.05, false );
      }
     }
    }
    
    if( !bErase )
    {
     newFrontier.push_back(updatedNeighborArc);
    }
   }
   
   // Clear waiting status
   if( neighbors[i]->isWaitingForAgent(getID()) )
   {
    neighbors[i]->clearWaitingForAgent();
   }
   
   // Inform neighbors of updated frontiers
   neighbors[i]->setFrontier( newFrontier );
  }
 }
}


void PursuitAgent::postProcessOurFrontier( vector<Frontier > &ourNeighborFrontiers, Pose &curPose, bool bDebugDisplay )
{
 // Special labels for endpoints of free arcs so that other agents can tell when our arcs actually ends on an obstacle
 LSRBound lsrBound = m_LSR.lsrBound();
 for( int i = 0; i < m_freeArcs.size(); i++ )
 {
  if( m_freeArcs[i].size() < lsrBound.size() )
  {
   m_LSR.getCell(m_freeArcs[i][0])->setIsLabelA();
   m_LSR.getCell(m_freeArcs[i][m_freeArcs[i].size()-1])->setIsLabelB();
   
   if( bDebugDisplay ) drawX( gColorGray, m_LSR.getCell(m_freeArcs[i][0])->point() + curPose.pos(), 0.05 );
   if( bDebugDisplay ) drawX( gColorBlue, m_LSR.getCell(m_freeArcs[i][m_freeArcs[i].size()-1])->point() + curPose.pos(), 0.05 );
  }
 }
 
 // This is to catch errors where we erase a neighbors obstacle end point, but
 // because of quantization our arc doesn't quite hit the obstacle theirs did
 for( int i = 0; i < m_frontier.size(); i++ )
 {
  if( m_frontier[i].size() < lsrBound.size() )
  {
   Cell* pCell1 = m_LSR.getCell(m_frontier[i][0]);
   Cell* pCell2 = m_LSR.getCell(m_frontier[i][m_frontier[i].size()-1]);
   
   if( gDebug >= 9 )
   {
    drawSquare( gColorBlue, m_LSR.center().pos() + pCell1->point(), 0.02, false );
    drawSquare( gColorGray, m_LSR.center().pos() + pCell2->point(), 0.02, false );
   }
   
   if( !pCell1->isLabelA() )
   {
    bool bSet = true;
    
    vector<Cell* > nCells = pCell1->neighbours();
    
    /*for( int l = 0; l < nCells.size(); l++ )
    {
     if( m_LSR.getCell(nCells[l]->indexes())->isLabelA() )
     {
      bSet = false;
     }
    }*/
   
    for( int m = 0; (bSet && m < ourNeighborFrontiers.size()); m++ )
    {
     for( int j = 0; (bSet && j < ourNeighborFrontiers[m].size()); j++ )
     {
      FrontierArc ourNeighborArc = ourNeighborFrontiers[m][j];
      
      for( int k = 0; (bSet && k < ourNeighborArc.size()); k++ )
      {
       for( int l = 0; l < nCells.size(); l++ )
       {
        if( ourNeighborArc[k] == nCells[l]->indexes() )
        {
         // Is this cell a part of a neighbor arc (ie, we haven't cleared it)
         if( !(m_LSR.getCell(nCells[l]->indexes())->isLSR()) && !(m_LSR.getCell(nCells[l]->indexes())->isObstacle()) )
         {
          if( gDebug >= 9 ) drawX( gColorGray, m_LSR.center().pos() + m_LSR.getCell(nCells[l]->indexes())->point(), 0.03 );
          bSet = false;
          break;
         }
        }
       }
      }
     }
    }
    
    if( bSet )
    {
     if( gDebug > 7 ) cout<< getID() << " setting error correction obstacle begin point for frontier " << i << " of size " << m_frontier[i].size() << endl;
     pCell1->setIsLabelA();
     if( gDebug >= 9 ) drawX( gColorGreen, m_LSR.center().pos() + pCell1->point(), 0.05 );
    }
   }
   
   if( !pCell2->isLabelB() )
   {
    bool bSet = true;
    
    vector<Cell* > nCells = pCell2->neighbours();
    
    /*for( int l = 0; l < nCells.size(); l++ )
    {
     if( m_LSR.getCell(nCells[l]->indexes())->isLabelB() )
     {
      bSet = false;
     }
    }*/
   
    for( int m = 0; (bSet && m < ourNeighborFrontiers.size()); m++ )
    {
     for( int j = 0; (bSet && j < ourNeighborFrontiers[m].size()); j++ )
     {
      FrontierArc ourNeighborArc = ourNeighborFrontiers[m][j];
      
      for( int k = 0; (bSet && k < ourNeighborArc.size()); k++ )
      {
       for( int l = 0; l < nCells.size(); l++ )
       {
        if( ourNeighborArc[k] == nCells[l]->indexes() )
        {
         // Is this cell a part of a neighbor arc (ie, we haven't cleared it)
         if( !(m_LSR.getCell(nCells[l]->indexes())->isLSR()) && !(m_LSR.getCell(nCells[l]->indexes())->isObstacle()) )
         {
          if( gDebug >= 9 ) drawX( gColorGray, m_LSR.center().pos() + m_LSR.getCell(nCells[l]->indexes())->point(), 0.03 );
          bSet = false;
          break;
         }
        }
       }
      }
     }
    }
    
    if( bSet )
    {
     if( gDebug > 7 ) cout<< getID() << " setting error correction obstacle begin point for frontier " << i << " of size " << m_frontier[i].size() << endl;
     pCell2->setIsLabelB();
     if( gDebug >= 9 ) drawX( gColorRed, m_LSR.center().pos() + pCell2->point(), 0.05 );
    }
   }
  }
 }
 
 // Fix stage errors where a ray sneaks into the corner of an obstacle
 Frontier newFrontier;
 for( int i = 0; i < m_frontier.size(); i++ )
 {
  bool bKill = false;
  int iLSRCount = 0;
  FrontierArc arc = m_frontier[i];
  
  /*
  if( arc.size() < gMinArcSize )
  {
   // Kill pointless single cell arcs
   bKill = true;
   vector<Cell* > nCells = m_LSR.getCell(arc[0])->neighbours();
   
   for( int m = 0; (bKill && m < ourNeighborFrontiers.size()); m++ )
   {
    for( int j = 0; (bKill && j < ourNeighborFrontiers[m].size()); j++ )
    {
     FrontierArc ourNeighborArc = ourNeighborFrontiers[m][j];
     
     for( int k = 0; (bKill && k < ourNeighborArc.size()); k++ )
     {
      for( int l = 0; l < nCells.size(); l++ )
      {
       if( ourNeighborArc[k] == nCells[l]->indexes() )
       {
        if( !(m_LSR.getCell(ourNeighborArc[k])->isLSR()) && !(m_LSR.getCell(ourNeighborArc[k])->isObstacle()) )
        {
         bKill = false;
         break;
        }
       }
      }
     }
    }
   }
   
   nCells = m_LSR.getCell(arc[arc.size()-1])->neighbours();
   
   for( int m = 0; (bKill && m < ourNeighborFrontiers.size()); m++ )
   {
    for( int j = 0; (bKill && j < ourNeighborFrontiers[m].size()); j++ )
    {
     FrontierArc ourNeighborArc = ourNeighborFrontiers[m][j];
     
     for( int k = 0; (bKill && k < ourNeighborArc.size()); k++ )
     {
      for( int l = 0; l < nCells.size(); l++ )
      {
       if( ourNeighborArc[k] == nCells[l]->indexes() )
       {
        if( !(m_LSR.getCell(ourNeighborArc[k])->isLSR()) && !(m_LSR.getCell(ourNeighborArc[k])->isObstacle()) )
        {
         bKill = false;
         break;
        }
       }
      }
     }
    }
   }
  }
  */
  
  if( arc.size() <= 3 )
  {
   if( m_LSR.getCell(arc[0])->isLabelA() && m_LSR.getCell(arc[arc.size()-1])->isLabelB() )
   {
    bKill = true;
   }
  }
  
  if( !bKill )
  {
   for( int j = 0; j < arc.size(); j++ )
   {
    if( !(m_LSR.getCell(arc[j])->isObstacle()) )
    {
     vector<Cell* > nCells = m_LSR.getCell(arc[j])->neighbours();
       
     // To handle Stage errors where one or two rays sneak into an obstacle
     for( int k = 0; k < nCells.size(); k++ )
     {
      if( nCells[k]->isLSR() && !(nCells[k]->isBoundary()) )
      {
       iLSRCount++;
       break;
      }
     }
    }
   }
   
   bKill = (iLSRCount <= std::max(0, (arc.size()-2)/2));
  }
  
  if( !bKill )
  {
   newFrontier.push_back(arc);
  }
  else
  {
   if( gDebug > 0 ) cout<< getID() << " erasing erroneous frontier" << endl;
  }
 }
 m_frontier = newFrontier;
}


void PursuitAgent::updateFrontier( AgentVector neighbors, bool bDebugDisplay )
{
 if( isTraveling() )//|| (getBehavior() == EXPAND && m_frontier.size() == 0) )
 {
  return;
 }
 
 if( m_bFrontierInitialized )
 {
  // Only run this function when first processing LSR from new viewpoint
  return;
 }
 
 if( gDebug > 7 ) cout<< "    " << getID() << " updating frontier" << endl;
 
 int bWaitForKeyboard = (gDebug > 9);
 
 player_color_t color;
 
 // Only update frontier and do arc coloring on the first update
 setFrontier( m_LSR.frontier(), true );
 m_bFrontierInitialized = true;
 m_freeArcs.clear();
 Pose curPose = getLSRPose();

 // Store full neighbor frontier arcs in our grid for use in classifying our LSR boundary
 vector<Frontier > ourNeighborFrontiers;
 // Store neighbor frontier arcs minus pieces in our LSR to send back to neighbors after we finish setting up our frontier
 vector<Frontier > updatedNeighborFrontiers;
 
 // The beginning/end cells of a frontier arc on an obstacle are very important for boundary classification
 // List of cells where a neighbor frontier arc begins on an obstacle
 vector<DubInt > neighborObstacleEndpoints;
 // List of cells where a neighbor frontier arc ends on an obstacle
 vector<DubInt > neighborObstacleBeginpoints;
 
 
 getNeighborFrontiers( neighbors, curPose, ourNeighborFrontiers, updatedNeighborFrontiers, neighborObstacleEndpoints, neighborObstacleBeginpoints );
 
 m_prevFrontier.clear();
 m_prevLSR = Grid();
 
 // At this point in the function we have translated all of neighbors' arcs into our
 // grid, so we are ready to go on to classifying our LSR boundary.  We have also determined
 // which pieces of our neighbors' arcs are inside our LSR, but will not transmit this
 // update back to our neighbors until our frontier is set up.
 
 if( bDebugDisplay )
 {
  if( gDebug > 5 ) cout<< "  " << m_frontier.size() << " our frontier arcs" << endl;
  if( gDebug > 5 ) cout<< "  " << ourNeighborFrontiers.size() << " neighbor frontiers" << endl;
  for( int i = 0; i < ourNeighborFrontiers.size(); i++ )
  {
   if( gDebug > 5 ) cout<< "  " << ourNeighborFrontiers[i].size() << " neighbor frontier arcs" << endl;
   for( int j = 0; j < ourNeighborFrontiers[i].size(); j++ )
   {
    FrontierArc ourNeighborArc = ourNeighborFrontiers[i][j];
    if( gDebug > 5 ) 
    {
     cout<< "  " << ourNeighborArc.size() << " neighbor frontier arc cells, from " ;
     cout<< ourNeighborArc[0].print() << " to " << ourNeighborArc[ourNeighborArc.size()-1].print() << endl;
    }
    
    for( int k = 0; k < ourNeighborArc.size(); k++ )
    {
     Position arcCellPos = getCellPosition(ourNeighborArc[k]);
     Position globalPos = curPose.pos() + arcCellPos;
     
     color.alpha = 0;
     color.red = 175;
     color.green = 175;
     color.blue = 175;
     
     drawSquare( color, globalPos, 0.01, true );
    }
    
    drawRedX( curPose.pos() + getCellPosition(ourNeighborArc[ourNeighborArc.size()-1]), 0.015 );
   }
  }
  
  for( int i = 0; i < neighborObstacleBeginpoints.size(); i++ )
  {
   drawX( gColorGreen, curPose.pos() + getCellPosition(neighborObstacleBeginpoints[i]), 0.05 );
  }
  
  for( int i = 0; i < neighborObstacleEndpoints.size(); i++ )
  {
   drawRedX( curPose.pos() + getCellPosition(neighborObstacleEndpoints[i]), 0.05 );
  }
  
  for( int i = 0; (i < m_frontier.size()); i++ )
  {
   FrontierArc arc = m_frontier[i];
   for( int j = 0; (j < arc.size()); j++ )
   {
    Position arcCellPos = getCellPosition(arc[j]);
    Position globalPos = curPose.pos() + arcCellPos;
    
    color.alpha = 0;
    color.red = 255;
    color.green = 175;
    color.blue = 255;
    
    drawSquare( color, globalPos, 0.01, true );
    
    if( j == 0 )
    {
     drawX( gColorGreen, curPose.pos() + getCellPosition(arc[j]), 0.05 );
    }
    else if( j == arc.size() - 1 )
    {
     drawRedX( curPose.pos() + getCellPosition(arc[j]), 0.05 );
    }
   }
  }
 }
 
 if( bDebugDisplay )
 {
  if( bWaitForKeyboard )
  {
   cout<< "Waiting for go ahead..." << endl;
   char input[10];
   cin>> input;
  }
  else
  {
   TimeUtilities::sleep( Time(1,500000) );
  }
  
  //gGP->Clear();
 }


 classifyOurFrontier( neighbors, curPose, ourNeighborFrontiers, neighborObstacleEndpoints, neighborObstacleBeginpoints, bDebugDisplay );
 
 
 updateNeighborFrontiers( neighbors, curPose, updatedNeighborFrontiers );
 
 
 postProcessOurFrontier( ourNeighborFrontiers, curPose, bDebugDisplay );
 
 gIterCount++;

 // Logging
 if( gLogFile.is_open() )
 {
  if( gGlobalGrid != NULL )
  {
   Position lsrPos = getLSRPose().pos();
   for( int i = 0; i < getLSRSize(); i++ )
   {
    if( getCell(i)->isLSR() )
    {
     Position LSRpoint = lsrPos + getCell(i)->point();
     
     Cell* pCell = gGlobalGrid->getCell( LSRpoint );
     
     if( !pCell->isLSR() )
     {
      pCell->setIsLSR();
      gGlobalGrid->_lsr.push_back(pCell->indexes());
     }
    }
   }
  }
  
  logData();
 }
 
 if( bDebugDisplay )
 {
  /*if( gDebug > 20 )
  {
   Position curPos = getLSRPosition();
   int iCount = 0;
   for( int i = 0; i < m_frontier.size(); i++ )
   {
    for( int j = 0; j < m_frontier[i].size(); j++ )
    {
     drawSquare( gColorFrontier, getCellPosition(m_frontier[i][j]) + curPos, 0.005, true );
     
     if( iCount%5 == 4 )
     {
      cout<< "Waiting for go ahead..." << endl;
      char input[10];
      cin>> input;
     }
     
     iCount++;
    }
   }
  }
  else */if( bWaitForKeyboard )
  {
   cout<< "Waiting for go ahead..." << endl;
   char input[10];
   cin>> input;
  }
  else
  {
   TimeUtilities::sleep( Time(2,500000) );
  }
  
  gGP->Clear();
 }
 
 /*
 if( bDebugDisplay )
 {
  for( int i = 0; i < m_freeArcs.size(); i++ )
  {
   for( int j = 0; j < m_freeArcs[i].size(); j++ )
   {
    color.alpha = 0;
    color.red = 255;
    color.green = 255;
    color.blue = 0;
    
    Position globalPos = getCell(m_freeArcs[i][j])->point() + curPos;
    
    drawSquare( color, globalPos, 0.01, true );
   }
  }
  
  if( bWaitForKeyboard )
  {
   cout<< "Waiting for go ahead..." << endl;
   char input[10];
   cin>> input;
  }
  else
  {
   TimeUtilities::sleep( Time(1,500000) );
  }
  
  gGP->Clear();
 }*/
 
}

ExplorationParams PursuitAgent::getExplParams( )
{
 double maxRange = getSensorRange();
 return ExplorationParams( maxRange, (m_pLaserProxy->GetMaxAngle() - m_pLaserProxy->GetMinAngle()), m_pLaserProxy->GetCount(), m_pLaserProxy->GetScanRes());
}

void PursuitAgent::recordLSR()
{
 // TODO: what is good size of final grid?
  
 m_LSR = Grid( getExplParams(), getScan(), /*Pose(getPosition(), 0)*/ getPose(), 2*getSensorRange(), 2*getSensorRange(), 2*getWidth() );
 
 m_LSR.defineBoard( gMinArcSize, gMinRadialArcSize, true );
 
 cout<< "  Cover field of view " << getCoverFieldOfView() << endl;
}

double PursuitAgent::getCoverRange()
{
 return (gCoverDistScalar * (getSensorRange() - getLSRResolution()));
}

double PursuitAgent::getCoverFieldOfView()
{
 return gSafeAngleFrac*(m_pLaserProxy->GetMaxAngle() - m_pLaserProxy->GetMinAngle());
}

bool PursuitAgent::isCellCovered( Pose viewPoint, DubInt iCell, bool bSilent )
{
 Position relPos = getCellPosition(iCell) - viewPoint.pos();
 if(!bSilent) cout<< "  relPos : " << relPos.print() << "  bearing  " << relPos.bearing().print() << endl;
 if( relPos.norm() > (getCoverRange() + 0.01) )
 {
  if(!bSilent) cout<< "    Distance fail" << endl;
  return false;
 }
 
 if( fabs(viewPoint.ori().alDiff(relPos.bearing())) > (getCoverFieldOfView()/2.0 + 0.01) )
 {
  if(!bSilent)
  {
   cout<< "    Angle fail, " << viewPoint.ori().print() << " vs " << relPos.bearing().print() << endl;
   cout<< "    " << fabs(viewPoint.ori().alDiff(relPos.bearing())) << "   " << getCoverFieldOfView()/2.0 << endl;
  }
  
  return false;
 }
 
 vector<Cell* > rasterLine = getRaster( getCell(viewPoint.pos())->indexes(), iCell );
 
 // Last cell on raster is target cell, allow raster to graze obstacle
 int iObstacleCount = 0;
 for( int i = 0; i < rasterLine.size() - 2; i++ )
 {
  if( !(rasterLine[i]->isLSR()) )
  {
   return false;
  }
  
  if( rasterLine[i]->isObstacle() )
  {
   iObstacleCount++;
  }
 }
 
 if( iObstacleCount > 1 )
 {
  if(!bSilent) cout<< "    Obstacle fail" << endl;
  return false;
 }
 
 if(!bSilent) cout<< "   covered" << endl;
 return true;
}

Pose PursuitAgent::pickBestViewPoint( int *iNumViewPoints, bool bDebugDisplay )
{
 Frontier frontier = getFrontier();
 assert( frontier.size() > 0 );
 assert( m_freeArcs.size() > 0 );
 
 if( gDebug > 2 ) cout<< "Picking best view point" << endl;
 
 (*iNumViewPoints) = 1;
 
 Pose viewPoint( 0.0, 0.0, 0.0 );
 //FrontierArc arc = frontier[m_followers.size()%frontier.size()];
 int iRelevantFreeArcs = 0;
 FrontierArc mostRelevantFreeArc;
 int iMaxRelevancy = 0;
 vector<DubInt > frontierCellsOnMRFA;
 
 // TODO: special handling so that arc behind robot is always MRFA when it exists
 
 // Determine most relevant free Arc
 for( int i = 0; i < m_freeArcs.size(); i++ )
 {
  FrontierArc freeArc = m_freeArcs[i];
  int iRelevancy = 0;
  vector<DubInt > frontierCellsOnFreeArc;
  
  for( int k = 0; k < frontier.size(); k++ )
  {
   for( int l = 0; l < frontier[k].size(); l++ )
   {
    for( int j = 0; j < freeArc.size(); j++ )
    {
     // TODO: There must be a smarter way to do this ... this scales worst case as (boundary size)^2
     if( freeArc[j] == frontier[k][l] )
     {
      iRelevancy++;
      frontierCellsOnFreeArc.push_back( freeArc[j] );
     }
    }
   }
  }
  
  if( gDebug > 5 ) cout << "  Free arc " << i << " has " << frontierCellsOnFreeArc.size() << " frontier cells out of " << freeArc.size() << endl;
  
  if( iRelevancy > 0 )
  {
   iRelevantFreeArcs++;
   
   if( iRelevancy > iMaxRelevancy )
   {
    mostRelevantFreeArc = m_freeArcs[i];
    frontierCellsOnMRFA = frontierCellsOnFreeArc;
    iMaxRelevancy = iRelevancy;
   }
  }
 }
 
 if( mostRelevantFreeArc.size() == 0 || frontierCellsOnMRFA.size() == 0 )
 {
  cerr<< "*** Error! *** Empty most relevant free arc, skipping agent " << getID() << endl;
  return getLSRPose();
 }
 
 if( gDebug > 2 )
 {
  cout<< "  Most relevant free arc: size " << mostRelevantFreeArc.size() << ", " << frontierCellsOnMRFA.size() << " frontier cells" << endl;
 }
 
 Pose curPose = getLSRPose();
 
 if( gDebug > 20 )
 {
  gGP->Clear();
  
  for( int i = 0; i < frontierCellsOnMRFA.size(); i++ )
  {
   drawSquare( gColorFrontier, getCellPosition(frontierCellsOnMRFA[i]) + curPose.pos(), 0.01, true );
   
   if( i%5 == 4 )
   {
    cout<< "Waiting for go ahead..." << endl;
    char input[10];
    cin>> input;
   }
  }
  
 }
 
 // Pick best view point on mostRelevantFreeArc
 int iMaxFrontierCoverage = 0;
 int iViewPoint = -1;
 
 // Calculate how many view points are necessary
 double angWidthOfMRFA = 0.0;
 
 Position frontierRight = getCellPosition(frontierCellsOnMRFA[0]);
 Angle rightAngle = atan2( frontierRight.y(), frontierRight.x() );
 Position frontierLeft = getCellPosition(frontierCellsOnMRFA.back());
 Angle leftAngle = atan2( frontierLeft.y(), frontierLeft.x() );
 
 angWidthOfMRFA = angDistCCW( rightAngle.dCastPi(), leftAngle.dCastPi(), (frontierCellsOnMRFA.size() <= (int)(getSensorRange()/(getLSRResolution() + 0.01))) );
 //angWidthOfMRFA = rightAngle.ccwDist(leftAngle);
 angWidthOfMRFA *= gOverlapFactor;
 
 // TODO: what's the right formula, given discretization and safety?
 double maxVPAngCoverage = 1.70 * acos( limit( gMinLength/(2*getCoverRange() - getWidth() - getLSRResolution()), 0.03, 0.49 ) );
 
 (*iNumViewPoints) = 1;
 
 for( int j = 0; j < frontierCellsOnMRFA.size(); j++ )
 {
  Position pos = getCellPosition(frontierCellsOnMRFA[j]);
  Angle angle = atan2( pos.y(), pos.x() );
  
  if( angDistCCW( rightAngle.dCastPi(), angle.dCastPi(), true )*gOverlapFactor > maxVPAngCoverage )
  {
   (*iNumViewPoints)++;
   rightAngle = angle;
  }
 }
 
 if( angWidthOfMRFA >= 2*M_PI - 0.01 )
 {
  (*iNumViewPoints) = 3;
 }
 
 rightAngle = atan2( frontierRight.y(), frontierRight.x() );
 
 //int iNumViewPoints = 1 + (angWidthOfMRFA/std::max(maxVPAngCoverage, (2*M_PI)/3));
 
 if( gDebug > 2 ) cout << "  VPs: " << (*iNumViewPoints) << " needed to cover MRFA with width " << angWidthOfMRFA << endl;
 if( gDebug > 5 ) cout << "       " << rightAngle.dCastPi() << " right, " << leftAngle.dCastPi() << " left" << endl;
 if( gDebug > 5 ) cout << "       (" << maxVPAngCoverage << " max coverage per VP)" << endl;
 
 // Loop over all LRR cells, finding point guaranteeing coverage of required fraction of MRFA and furthest away
 LRR curLRR = getLSR()._lrr;
 double bestDistHeuristic = -1.0;
 double bestCoverage = 0.0;
 double minVPDist = std::min( std::max(gMinLength, 1.2*getWidth()), getSensorRange() - getWidth() - getLSRResolution());
 minVPDist = std::max( minVPDist, (double)2*getLSRResolution());
 
 if( gDebug > 5 ) cout << "       " << minVPDist << " minimum VP dist" << endl;
 
 frontierRight = getCellPosition(frontierCellsOnMRFA[0]);
 frontierLeft = frontierRight;
 
 if( gDebug > 10 ) cout<< "      Must cover point " << getCellPosition(frontierCellsOnMRFA[0]).print() << endl;
 
 for( int i = 0; i < curLRR.size(); i++ )
 {
  if( getCell(curLRR[i])->isBoundary() )
  {
   continue;
  }
  
  int iFrontierCoverage = 0;
  double distHeuristic = 0.0;
  Angle rightAngleCovered = rightAngle;
  Angle leftAngleCovered = rightAngle;
  frontierLeft = frontierRight;
  double angCoverage = 0.0;
  Position baseVec = getCellPosition(frontierCellsOnMRFA[0]) - getCellPosition(curLRR[i]);
  Position rightVec = frontierRight - getCellPosition(curLRR[i]);
  Position leftVec = frontierLeft - getCellPosition(curLRR[i]);
  Pose tempViewPoint = Pose( getCellPosition(curLRR[i]), getCellPosition(curLRR[i]).bearing() );
  
  if( gDebug > 19 )
  {
   //cout<< "      test VP pose: " << tempViewPoint.print() << endl;
   //cout<< "      right vec: " << rightVec.print() << " bearing " << rightVec.bearing().print() << endl;
  }
  
  if( !(isCellCovered( tempViewPoint, frontierCellsOnMRFA[0])) )
  {
   // must cover right most frontier cell, to avoid splitting frontier
   continue;
  }
  
  //cout<< "covered! waiting for continue ..." << endl;
  //getchar();
  
  if( getCell(curLRR[i])->distVP() < minVPDist )
  {
   // must be further away than min length
   continue;
  }
  
  if( (m_followers.size() > 0 || isWaitingForAnyAgent()) )
  {
   // Guarantee required coverage of MRFA
   bool bAllCovered = true;
   bool bAngCoverage = false;
   //if( gDebug > 12 ) cout<< "     considering cell " << i << endl;
   
   for( int j = 0; j < frontierCellsOnMRFA.size(); j++ )
   {
    if( isCellCovered(tempViewPoint, frontierCellsOnMRFA[j]) )
    {
     Position pos = getCellPosition(frontierCellsOnMRFA[j]);
     frontierLeft = pos;
     Angle angle = atan2( pos.y(), pos.x() );
     leftAngleCovered = angle;
     
     iFrontierCoverage++;
     distHeuristic += getCellPosition(curLRR[i]).dist(pos);
     
     angCoverage = angDistCCW( rightAngleCovered.dCastPi(), leftAngleCovered.dCastPi(), true );
     
     if( (*iNumViewPoints) > 1 && (angCoverage) > (angWidthOfMRFA / (*iNumViewPoints)) )
     {
      break;
     }
    }
    else
    {
     //if( iFrontierCoverage > 0 && gDebug > 12 ) cout<< "     " << i << " only covers " << iFrontierCoverage << endl;
     bAllCovered = false;
     break;
    }
   }
   
   if( (*iNumViewPoints) == 1 )
   {
    if( iFrontierCoverage >= iMaxFrontierCoverage )
    {
     if( (iFrontierCoverage > iMaxFrontierCoverage) || (distHeuristic < bestDistHeuristic) || (bestDistHeuristic < 0.0) )
     {
      if( gDebug > 9 ) cout<< "     New best VP at cell " << i << ", covers " << iFrontierCoverage << " with dist heur " << distHeuristic << endl;;
      iMaxFrontierCoverage = iFrontierCoverage;
      iViewPoint = i;
      
      Position rightVec = frontierRight - getCellPosition(curLRR[i]);
      Position leftVec = frontierLeft - getCellPosition(curLRR[i]);

      viewPoint = Pose( getCellPosition(curLRR[i]), rightVec.bearing() + std::min(getCoverFieldOfView()/2.0, (rightVec.bearing().ccwDiff(leftVec.bearing())/2.0)) );
      bestDistHeuristic = distHeuristic;
     }
    }
   }
   else
   {
    if( (iFrontierCoverage >= iMaxFrontierCoverage) )
    {
     if( (iFrontierCoverage > iMaxFrontierCoverage) || (distHeuristic < bestDistHeuristic) || (bestDistHeuristic < 0.0) )
     {
      if( gDebug > 9 ) cout<< "     New best VP at cell " << i << ", covers " << iFrontierCoverage << " (" << angCoverage << ") with dist heur " << distHeuristic << endl;
      iMaxFrontierCoverage = iFrontierCoverage;
      bestCoverage = angCoverage;
      iViewPoint = i;
      
      Position rightVec = frontierRight - getCellPosition(curLRR[i]);
      Position leftVec = frontierLeft - getCellPosition(curLRR[i]);
      
      viewPoint = Pose( getCellPosition(curLRR[i]), rightVec.bearing() + std::min(getCoverFieldOfView()/2.0, (rightVec.bearing().ccwDiff(leftVec.bearing())/2.0)) );
      bestDistHeuristic = distHeuristic;
     }
    }
   }
  }
  else
  {
   
   // Cover all of frontier
   for( int j = 0; j < frontier.size(); j++ )
   {
    for( int k = 0; k < frontier[j].size(); k++ )
    {
     if( isCellCovered( tempViewPoint, frontier[j][k] ) )
     {
      Position vec = getCellPosition(frontier[j][k]) - getCellPosition(curLRR[i]);
      
      
      iFrontierCoverage++;
      distHeuristic += vec.norm();
     }
    }
   }
   
   if( (iFrontierCoverage > iMaxFrontierCoverage) || 
   ((iFrontierCoverage == iMaxFrontierCoverage) && (distHeuristic < bestDistHeuristic)) )
   {
    iMaxFrontierCoverage = iFrontierCoverage;
    iViewPoint = i;
    
    viewPoint = tempViewPoint;
    bestDistHeuristic = distHeuristic;
    
    if( gDebug > 15 ) cout<< "    new best: " << viewPoint.print() << " covers " << iMaxFrontierCoverage << " with heuristic " << bestDistHeuristic << endl;
   }
  }
 }
 
 // Does best view point cover all frontier cells?
 int iTotalFrontierCells = 0;
 for( int i = 0; i < frontier.size(); i++ )
 {
  iTotalFrontierCells += frontier[i].size();
 }
 
 if( gDebug > 2 ) cout<< "  View point " << viewPoint.print() << " covers " << iMaxFrontierCoverage << " out of " << iTotalFrontierCells << " frontier cells" << endl;
 
 if( iMaxFrontierCoverage < iTotalFrontierCells )
 {
  (*iNumViewPoints) = std::max( (*iNumViewPoints), 2 );
 }
 
 if( gDebug > 19 )
 {
  gGP->Clear();
  
  for( int j = 0; j < frontier.size(); j++ )
  {
   for( int k = 0; k < frontier[j].size(); k++ )
   {
    if( isCellCovered( viewPoint, frontier[j][k] ) )
    {
     drawSquare( gColorFrontier, getCellPosition(frontier[j][k]) + curPose.pos(), 0.01, true );
    }
    else
    {
     drawSquare( gColorGray, getCellPosition(frontier[j][k]) + curPose.pos(), 0.01, true );
    }
   }
  }
  
  cout<< "Waiting for continue ..." << endl;
  char input[10];
  cin>> input;
 }
 
 
 
 //if( bDebugDisplay )
 //{
  //gGP->Clear();
  
  //drawLSR( *gGP, false );
  
  //drawRedX( getLSRPose().pos() + viewPoint.pos(), .1 );
  
  //if( gDebug > 30 )
  //{
   //cout<< "Waiting for go ahead..." << endl;
   //char input[10];
   //cin>> input;
  //}
  //else
  //{
   //TimeUtilities::sleep( Time(1,000000) );
  //}
  
  //gGP->Clear();
 //}
 
 return viewPoint;
}

void PursuitAgent::setFrontier(Frontier frontier, bool bForce )
{
 if(bForce)
 {
  m_frontier = frontier;
  return;
 }
 
 int iCells = 0;
 for( int i = 0; i < frontier.size(); i++ )
 {
  iCells += frontier[i].size();
 }
 
 if( !m_bFrontierInitialized )
 {
  bool bDoUpdate = (iCells < getTotalFrontierCells());
  
  m_prevFrontier = frontier;
  
  if( bDoUpdate ) 
  {
   if( gDebug > 0 ) cout<< "  " << getID() << " doing set prev frontier updated" << endl;
   processFrontier( AgentVector() );
  }
 }
 else
 {
  bool bDoUpdate = (iCells == 0 || iCells < getTotalFrontierCells());
  
  m_frontier = frontier;
  
  if( bDoUpdate && isWaitingForAnyAgent() )
  {
   processFrontier( AgentVector() );
  }
  // if new frontier is empty, guard logic will switch behavior
 }
}

int PursuitAgent::getTotalFrontierCells()
{
 int iCells = 0;
 Frontier frontier = getFrontier();
 for( int i = 0; i < frontier.size(); i++ )
 {
  iCells += frontier[i].size();
 }
 
 return iCells;
}

Pose PursuitAgent::getViewpoint( )
{
 if( m_path.size() > 0 )
 {
  return m_path.back();
 }
 else
 {
  return getPose();
 }
}

Scan PursuitAgent::getScan( )
{
 clientRead();
 
 int iCount = m_pLaserProxy->GetCount();
 
 vector<Ray> rays;
 vector<Ray> rays2;
 
 for( int i = 0; i < iCount; i++ )
 {
  // TODO: any easy entry for bValid?
  rays.push_back( Ray( m_pLaserProxy->GetRange(i), m_pLaserProxy->GetBearing(i), /*bValid*/ true ) );
 }
 
 return Scan( rays, Time() );
}

vector<Cell* > PursuitAgent::getRaster( DubInt iCell1, DubInt iCell2 )
{
 if( m_bFrontierInitialized )
 {
  return m_LSR.bresenham( getCell(iCell1), getCell(iCell2) );
 }
 else
 {
  return m_prevLSR.bresenham( getCell(iCell1), getCell(iCell2) );
 }
}


Pose PursuitAgent::getLSRPose()
{
 if( m_bFrontierInitialized )
 {
  return m_LSR.center();
 }
 else
 {
  return m_prevLSR.center();
 }
}

void PursuitAgent::setToExpand( Path path, bool bClearPrevFrontier, AgentVector neighbors, bool bDoExpand )
{
 if( gDebug > 5 ) cout<< "  " << getID() << ":  set to EXPAND" << endl;
 
 m_eBehavior = EXPAND;
 
 m_LSR = Grid();
 m_frontier.clear();
 m_bFrontierInitialized = false;
 if( bClearPrevFrontier )
 {
  m_prevLSR = m_LSR;
  m_prevFrontier.clear();
 }
 //m_freeArcs.clear();
 m_path = path;
 
 if( getPosition().dist(path.back().pos()) > 0.01 || fabs(getPose().ori().alDiff(path.back().ori())) > (2*M_PI)/180)
 {
  goTo( path.back() );
 }
 
 m_pWaitForAgent = NULL;
 
 if( bDoExpand ) Expand( neighbors );
}

void PursuitAgent::setToGuard( AgentVector neighbors )
{
 if( gDebug > 5 ) cout<< "  " << getID() << ":  set to GUARD" << endl;
 
 m_eBehavior = GUARD;
 
 recordLSR();
 
 m_frontier.clear();
 m_bFrontierInitialized = false;
 //m_freeArcs.clear();
 //m_path.clear();
 
 m_pWaitForAgent = NULL;
 
 // Stop any current motion
 //goTo( getPose() );
 
 // preserve list of followers
 
 if( neighbors.size() > 0 )
 {
  Guard( neighbors );
 }
}

void PursuitAgent::setToFollow( AgentVector neighbors )
{
 if( gDebug > 5 ) cout<< "  " << getID() << ":  set to FOLLOW" << endl;
 
 m_eBehavior = FOLLOW;
 
 m_LSR = Grid();
 m_prevLSR = m_LSR;
 m_frontier.clear();
 m_prevFrontier.clear();
 m_bFrontierInitialized = false;
 m_freeArcs.clear();
 m_path.clear();
 
 m_pWaitForAgent = NULL;
 
 // Stop any current motion
 //goTo( getPose() );
 
 while( !m_followers.empty() )
 {
  // Should never happen ...
  m_followers.back()->setToWander(neighbors);
  m_followers.pop_back();
 }
}

void PursuitAgent::setToWander( AgentVector neighbors )
{
 if( gDebug > 5 ) cout<< "  " << getID() << ":  set to WANDER" << endl;
 
 m_eBehavior = WANDER;
 
 m_LSR = Grid();
 m_prevLSR = m_LSR;
 m_frontier.clear();
 m_prevFrontier.clear();
 m_bFrontierInitialized = false;
 m_freeArcs.clear();
 m_path.clear();
 
 m_pWaitForAgent = NULL;
 
 while( !m_followers.empty() )
 {
  m_followers.back()->setToWander( neighbors );
  m_followers.pop_back();
 }
 
 for( uint i = 0; i < neighbors.size(); i++ )
 {
  if( neighbors[i]->getID() != getID() )
  {
   if( neighbors[i]->isWaitingForAgent(getID()) )
   {
    neighbors[i]->clearWaitingForAgent();
   }
  }
 }
 
 // Stop any current motion
 //goTo( getPose() );
}

void PursuitAgent::setToNoBehavior( )
{
 if( gDebug > 5 ) cout<< "  " << getID() << ":  set to NO_BEHAVIOR" << endl;
 
 m_eBehavior = NO_BEHAVIOR;
 
 m_LSR = Grid();
 m_frontier.clear();
 m_freeArcs.clear();
 m_bFrontierInitialized = false;
 m_path.clear();
 
 m_pWaitForAgent = NULL;
 
 while( !m_followers.empty() )
 {
  m_followers.back()->setToNoBehavior();
  m_followers.pop_back();
 }
 
 // Stop any current motion
 goTo( getPose() );
}

void PursuitAgent::goTo( Pose pose )
{
 m_pPosProxy->GoTo((double)(pose.pos().x()),(double)(pose.pos().y()),(double)(pose.ori().dCast2Pi()));
 
 for( int i = 0; i < m_followers.size(); i++ )
 {
  Position followerPos = getFollowerPosition();
  followerPos *= (i+1.0);
  m_followers[i]->goTo( Pose(pose.pos() - followerPos, pose.ori()) );
 }
}

Position PursuitAgent::getFollowerPosition()
{
 Position pos = Position( 0, 0 );
 if( m_path.size() > 1 )
 {
  pos = (m_path.back().pos() - m_path[m_path.size()-2].pos());
  
  //double multiplier = std::min( 1.0/(m_followers.size() + 1), (1.5*getWidth())/(pos.norm() + 0.1) );
  double multiplier = (1.6*getWidth())/std::max(pos.norm(), 0.1);
  
  pos *= multiplier;
 }
 
 return pos;
}

void PursuitAgent::addFollower( PursuitAgent* newFollower, bool bMove )
{
 if( gDebug > 5 ) cout<< "  " << getID() << " gained follower " << newFollower->getID() << endl;
 
 if( newFollower->getID() == getID() )
 {
  assert(false);
  return;
 }
 
 m_followers.push_back(newFollower);
 
 if( bMove )
 {
  if( getPath().size() > 0 )
  {
   newFollower->goTo( Pose(getPath().back().pos() - getFollowerPosition()*m_followers.size(), getPath().back().ori()) );
  }
  else
  {
   //newFollower->goTo( getPose() );
  }
 }
}

void PursuitAgent::doCurrentBehavior( AgentVector neighbors )
{
 //if( gDebug > 8 ) cout<< "  " << getID() << ":  doing current behavior" << endl;
 AgentVector leaders;
 
 clientRead();
 
 switch (m_eBehavior) {
  case EXPAND:
   Expand( neighbors );
   break;
  case GUARD:
   Guard( neighbors );
   break;
  case FOLLOW:
   if( gDebug > 8 ) cout<< "  " << getID() << ":  following" << endl;
   break;
  case NO_BEHAVIOR:
   break;
  case WANDER:
   if( gDebug > 8 ) cout<< "  " << getID() << ":  wandering" << endl;
   Wander( neighbors );
   break;
  default:
   cerr<< "ERROR: No behavior category" << endl;
   assert(false);
  }
}

bool PursuitAgent::isLeader()
{
 return ((m_eBehavior == EXPAND) || (m_eBehavior == GUARD));
}

bool PursuitAgent::isReady()
{
 if( m_pPosProxy->GetSize().sw == 0 || m_pLaserProxy->GetCount() == 0 )
 {
  return false;
 }
 
 if( isTraveling() )
 {
  return false;
 }
 
 if( isWaitingForAnyAgent() )
 {
  return false;
 }
 
 return true;
}

bool PursuitAgent::isWaitingForAnyAgent()
{
 if( m_pWaitForAgent != NULL )
 {
  return true;
 }
 
 return false;
}

bool PursuitAgent::isWaitingForAgent(int iID)
{
 if( !isWaitingForAnyAgent() )
 { 
  return false;
 }
 
 if( m_pWaitForAgent->getID() == iID )
 {
  return true;
 }
 
 return false;
}

void PursuitAgent::clearWaitingForAgent()
{
 m_pWaitForAgent = NULL;
}

bool PursuitAgent::isTraveling()
{
 if( m_path.empty() )
 {
  return false;
 }
 
 clientRead();
 
 Pose curPose = getPose();
 double dist = curPose.pos().dist(m_path.back().pos());
 double check = std::max(m_pPosProxy->GetSize().sw/4,m_pPosProxy->GetSize().sl/4);
 
 check = std::max( check, 0.011 );
 
 if( dist > check )
 {
  //if( dist > 4*check || ((m_pPosProxy->GetXSpeed() + m_pPosProxy->GetYSpeed()) > check/10.0 || m_pPosProxy->GetYawSpeed() > 0.05) )
  {
   if( gDebug > 10 ) cout<< "    " << getID() << " is travelling, at " << curPose.print() << " dist " << dist << " v. check " << check << endl;
   return true;
  }
 }
 
 if( (m_pLaserProxy->GetMaxAngle() - m_pLaserProxy->GetMinAngle()) < 1.9*M_PI )
 {
  if( fabs(curPose.ori().alDiff( m_path.back().ori() )) > (2 * M_PI)/180 )
  {
   if( gDebug > 10 ) cout<< "    " << getID() << " is turning to final pose, at " << curPose.print() << endl;
   return true;
  }
 }
 
 return false;
}

bool PursuitAgent::canDetectTargetPosition( Position targetPos )
{
 Position curPos = getLSRPose().pos();
 
 if( m_bFrontierInitialized || m_prevFrontier.size() > 0 )
 {
  Cell* targetCell = getCell( targetPos - curPos );
  
  if( targetCell != NULL )
  {
   if( targetCell->isLSR() || targetCell->isObstacle() )
   {
    if( gDebug > 0 ) cout<< "Detection!  Agent " << getID() << " at " << curPos.print() << " spots evader at " << targetPos.print() << endl;
    return true;
   }
  }
 }
 
 return false;
}

void PursuitAgent::drawBoundary( Graphics2dProxy &gp )
{
 bool bDoAgentColors = (gDebug > 10);
 Position curPos = getLSRPose().pos();

 LSRBound lsrBound = getLSR().lsrBound();
 Frontier frontier = getFrontier();
 int i, j, l;
 Cell* pBoundCell;
 bool bIsActualFrontier;
 
 if( frontier.size() <= 0 )
 {
  return;
 }
 
 for( l = 0; l < lsrBound.size(); l++ )
 {
  pBoundCell = getCell(lsrBound[l]);
  
  bIsActualFrontier = false;
  for( i = 0; !bIsActualFrontier && i < frontier.size(); i++ )
  {
   for( j = 0; !bIsActualFrontier && j < frontier[i].size(); j++ )
   {
    bIsActualFrontier = (lsrBound[l] == frontier[i][j]);
   }
  }
  
  if( bIsActualFrontier )
  {
   drawSquare( (bDoAgentColors ? gColorVec[getID()] : gColorFrontier), curPos + pBoundCell->point(), 0.013, true );
   
   if( gDebug > 8 )
   {
    if( pBoundCell->isLabelA() )
    {
     drawX( gColorGreen, curPos + pBoundCell->point(), 0.02 );
    }
    else if( pBoundCell->isLabelB() )
    {
     drawX( gColorRed, curPos + pBoundCell->point(), 0.02 );
    }
   }
  }
  else if( pBoundCell->isFrontier() )
  {
   drawSquare( gColorFreeArc, curPos + pBoundCell->point(), 0.013, true );
  }
  else if( pBoundCell->isObstacle() )
  {
   drawSquare( gColorObstacle, curPos + pBoundCell->point(), 0.013, true );
  }
 }
 
 if( m_path.size() > 0 )
 {
  //drawX( gColorVec[getID()], m_path.back(), 0.05 );
 }
}

void PursuitAgent::drawFrontier( Graphics2dProxy &gp )
{
 bool bDoAgentColors = (gDebug > 8);
 Position curPos = getLSRPose().pos();
 player_point_2d_t point;
 
 Frontier frontier = getFrontier();
 
 for( int i = 0; i < frontier.size(); i++ )
 {
  player_point_2d_t frontierPts[frontier[i].size()];
  for( int j = 0; j < frontier[i].size(); j++ )
  {
   Cell* pCell = getCell(frontier[i][j]);
   if( pCell != NULL )
   {
    frontierPts[j].px = curPos.x() + pCell->point().x();
    frontierPts[j].py = curPos.y() + pCell->point().y();
    
    drawSquare( gColorVec[getID()], curPos + pCell->point(), 0.01, true );
   }
  }
  
  if( frontier[i].size() > 1 )
  {
   gp.Color( gColorVec[getID()] );
  
   gp.DrawPolyline( frontierPts, frontier[i].size() );
  }
  
  if( bDoAgentColors ) drawRedX( curPos + getCell(frontier[i][frontier[i].size()-1])->point(), .02 );
 }
 
 if( m_path.size() > 0 )
 {
  if( bDoAgentColors ) drawX( gColorVec[getID()], m_path.back().pos(), 0.05 );
 }
}

void PursuitAgent::drawLSR( Graphics2dProxy &gp, bool bOnlyLSR )
{
 Position curPos = getLSRPose().pos();
 player_point_2d_t point;
 
 int LSRsize = getLSRSize();
 player_color_t color;
 
 for( int i = 0; i < LSRsize; i++ )
 {
  Position LSRpoint = getCell(i)->point();
  
  if( !bOnlyLSR && getCell(i)->isFrontier() )
  {
   color.alpha = 0;
   color.red = 0;
   color.green = 0;
   color.blue = 255;

   drawSquare( color, curPos + LSRpoint, 0.01, true );
  }
  else if( !bOnlyLSR && getCell(i)->isObstacle() )
  {
   color.alpha = 0;
   color.red = 255;
   color.green = 0;
   color.blue = 0;

   drawSquare( color, curPos + LSRpoint, 0.01, true );
  }
  else if( !bOnlyLSR && getCell(i)->isLRR() )
  {
   color.alpha = 0;
   color.red = 0;
   color.green = 180;
   color.blue = 0;

   drawSquare( color, curPos + LSRpoint, 0.01, true );
  }
  else if( getCell(i)->isLSR() )
  {            
   color.alpha = 0;
   color.red = 0;
   color.green = 255;
   color.blue = 0;

   drawSquare( color, curPos + LSRpoint, 0.01, true );
  }
 }
 
}

#endif