#include <vector>
#include <queue>
#include <assert.h>
#include "parse.h"

using namespace std;

#undef DEBUG

#ifdef DEBUG
void printMap( string mapName, vector< vector<nodePair> > &map, int N )
{
  debug << "Map: " << mapName << endl;
  for (int i=0; i<N; i++)
  {
    for (int j=0; j<N; j++)
      debug << map[i][j].dist << "\t";
    debug << endl;
  }
}
#endif

void makeMatrix(vector<vector<int> > &listmap, vector< vector<nodePair> > &map, int N)
{
  // Initialize the maps to MAXDISTANCE everywhere
  for (int i=0; i<N; i++)
    for (int j=0; j<N; j++)
    {
      map[i][j].dist = MAXDISTANCE;
      if (i==j)
        map[i][j].dist = 0;
    }

  for (int i=0; i<N; i++)
    for (unsigned int j=0; j<listmap[i].size(); j++)
    {
      map[i][listmap[i][j]].dist = 1;
      map[i][listmap[i][j]].numPaths = 1;
      map[i][listmap[i][j]].initPath[0] = listmap[i][j];
    }
#if 0
  printMap("listmap",map,N);
#endif
}

/* Find all-pairs shortest distance with path tracking. Only keeps the initial
 * edge of each path, up to four per edge */
void floyd(vector< vector<nodePair> > &map, int N)
{
  for (int k=0; k<N; k++)
    for (int i=0; i<N; i++)
      for (int j=0; j<N; j++)
      {
        if (k == i || k == j)
          continue;
        if (map[i][j].dist > map[i][k].dist + map[k][j].dist)
        {
          map[i][j].dist = map[i][k].dist + map[k][j].dist;
          map[i][j].numPaths = map[i][k].numPaths;
          for (int l=0; l<map[i][k].numPaths; l++)
            map[i][j].initPath[l] = map[i][k].initPath[l];
        } else if (map[i][j].dist == map[i][k].dist + map[k][j].dist)
        {
          for (int l=0; l<map[i][k].numPaths; l++)
          {
            int alreadyThere = 0;
            for (int m=0; m<map[i][j].numPaths; m++)
              if (map[i][j].initPath[m] == map[i][k].initPath[l])
                alreadyThere = 1;
            if (!alreadyThere && map[i][j].numPaths < 4)
            {
              map[i][j].initPath[map[i][j].numPaths] = map[i][k].initPath[l];
              map[i][j].numPaths++;
            }
          }
        }
      }
}

/* Corrects police foot and car shortest path maps to account for the
 * fact that cops can switch between car and pedestrian modes when at HQ */
void policeFloyd(vector< vector<nodePair> > &carMap, vector< vector<nodePair> > &footMap, int N, int hq)
{
  for (int i=0; i<N; i++)
    for (int j=0; j<N; j++)
    {
      if (i == hq || j == hq)
        continue;
      if (carMap[i][j].dist > carMap[i][hq].dist + footMap[hq][j].dist)
      {
        carMap[i][j].dist = carMap[i][hq].dist + footMap[hq][j].dist;
        carMap[i][j].numPaths = carMap[i][hq].numPaths;
        for (int l=0; l<carMap[i][hq].numPaths; l++)
          carMap[i][j].initPath[l] = carMap[i][hq].initPath[l];
      } else if (carMap[i][j].dist == carMap[i][hq].dist + footMap[hq][j].dist)
      {
        for (int l=0; l<carMap[i][hq].numPaths; l++)
        {
          int alreadyThere = 0;
          for (int m=0; m<carMap[i][j].numPaths; m++)
            if (carMap[i][j].initPath[m] == carMap[i][hq].initPath[l])
              alreadyThere = 1;
          if (!alreadyThere && carMap[i][j].numPaths < 4)
          {
            carMap[i][j].initPath[carMap[i][j].numPaths] = 
              carMap[i][hq].initPath[l];
            carMap[i][j].numPaths++;
          }
        }
      }
    }

  for (int i=0; i<N; i++)
    for (int j=0; j<N; j++)
    {
      if (i == hq || j == hq)
        continue;
      if (footMap[i][j].dist > footMap[i][hq].dist + carMap[hq][j].dist)
      {
        footMap[i][j].dist = footMap[i][hq].dist + carMap[hq][j].dist;
        footMap[i][j].numPaths = footMap[i][hq].numPaths;
        for (int l=0; l<footMap[i][hq].numPaths; l++)
          footMap[i][j].initPath[l] = footMap[i][hq].initPath[l];
      } else if (footMap[i][j].dist == footMap[i][hq].dist + carMap[hq][j].dist)
      {
        for (int l=0; l<footMap[i][hq].numPaths; l++)
        {
          int alreadyThere = 0;
          for (int m=0; m<footMap[i][j].numPaths; m++)
            if (footMap[i][j].initPath[m] == footMap[i][hq].initPath[l])
              alreadyThere = 1;
          if (!alreadyThere && footMap[i][j].numPaths < 4)
          {
            footMap[i][j].initPath[footMap[i][j].numPaths] = 
              footMap[i][hq].initPath[l];
            footMap[i][j].numPaths++;
          }
        }
      }
    }
}

/* Check for a 100% safe minimum distance path for the robber from 
 * curPos to dest. Even if the police knew where he was they couldn't catch him
 * in time. Also returns the first edge of the path. */
bool safePath(vector< vector<nodePair> > &robberMap, 
	      vector< vector<nodePair> > &carMap, 
	      vector< vector<nodePair> > &footMap,
    int curPos, int dest, copsInfo cops, int &firstNode, int depth = 0)
{
#ifdef DEBUG
  debug << __FUNCTION__ << ": branching factor="
        << robberMap[curPos][dest].numPaths << endl;
#endif

  int garbage;
  for (int i=0; i<cops.numCops; i++)
  {
    if (cops.mode[i] == CAR)
    {
      if (carMap[cops.pos[i]][curPos].dist <= depth)
        return false;
    } else
    {
      if (footMap[cops.pos[i]][curPos].dist <= depth)
        return false;
    }
  }

  firstNode = curPos;
  if (dest == curPos)
    return true;

  for (int i=0; i<robberMap[curPos][dest].numPaths; i++)
  {
    if (safePath(robberMap, carMap, footMap, 
          robberMap[curPos][dest].initPath[i], dest, cops, garbage, depth+1))
    {
      firstNode = robberMap[curPos][dest].initPath[i];
      return true;
    }
  }
  
  return false;
}

/* Finds a minimum distance path from curPos to dest which maximizes the
 * minimum distance to a cop assuming all cops stay put. Returns the
 * min police distance along the best path, and the first node along
 * such a path. */
int avoidPolicePath(vector< vector<nodePair> > &robberMap, 
		    vector< vector<nodePair> > &carMap, 
		    vector< vector<nodePair> > &footMap,
    int curPos, int dest, copsInfo cops, int &firstNode)
{
  int garbage;
  int minDistFromHere = MAXDISTANCE;

  for (int i=0; i<cops.numCops; i++)
  {
    if (cops.pos[i] == curPos)
      return 0;
    if (cops.mode[i] == CAR)
    {
      if (carMap[cops.pos[i]][curPos].dist < minDistFromHere)
      {
        minDistFromHere = carMap[cops.pos[i]][curPos].dist;
        firstNode = robberMap[curPos][dest].initPath[0];
      }
    } else
    {
      if (footMap[cops.pos[i]][curPos].dist < minDistFromHere)
      {
        minDistFromHere = footMap[cops.pos[i]][curPos].dist;
        firstNode = robberMap[curPos][dest].initPath[0];
      }
    }
  }

  if (curPos == dest)
    return minDistFromHere;

  int maxmin = 0;
  for (int i=0; i<robberMap[curPos][dest].numPaths; i++)
  {
    int temp = avoidPolicePath(robberMap, carMap, footMap,
          robberMap[curPos][dest].initPath[i], dest, cops, garbage);
    if (temp > maxmin);
    {
      maxmin = temp;
      firstNode = robberMap[curPos][dest].initPath[i];
    }
  }

  return (minDistFromHere < maxmin ? minDistFromHere : maxmin);
}

/* The cop tries to intercept the robber on his way from one place to another.
 * Returns the move to make such that the cop ends up on the same node as
 * the robber assuming that the robber takes the shortest path. */
int interceptRobber(vector< vector<nodePair> > &robberMap, vector< vector<nodePair> > &copMap, int copPos,
    int robberPos, int target)
{
  for (int pos=robberPos; pos!=target; pos=robberMap[pos][target].initPath[0])
  {
    if (robberMap[robberPos][pos].dist >= copMap[copPos][pos].dist)
      return copMap[copPos][pos].initPath[0];
  }
  // shouldn't get here??? -keir
  assert(0);
  return 0;
}

/* Look for a path from orig to dest with a set of disallowed nodes. */
int findClearPath(int orig, vector<bool>& targets, World &w, vector<vector<int> > &map,
    vector<bool>& badNodes, vector<int> &moveSeq, int numTries)
{
  if (targets[orig])
    return 0;
  vector<bool> dontGoThere = badNodes;
  dontGoThere[orig] = true;

  queue<int> nodeQ, lengthQ;
  queue<vector<int> > routeQ;
  for (unsigned int i=0; i<map[orig].size(); i++)
    if (!dontGoThere[map[orig][i]])
    {
      nodeQ.push(map[orig][i]);
      routeQ.push(vector<int>());
      routeQ.back().push_back(map[orig][i]);
      lengthQ.push(1);
      dontGoThere[map[orig][i]] = true;
    }
 
  while (!nodeQ.empty())
  {
    int curNode = nodeQ.front();
    if (targets[curNode])
    {
      numTries--;
      if (numTries <= 0)
      {
        moveSeq = routeQ.front();
        return lengthQ.front();
      }
    }
    for (unsigned int i=0; i<map[curNode].size(); i++)
      if (!dontGoThere[map[curNode][i]])
      {
        nodeQ.push(map[curNode][i]);
        vector<int> newRoute = routeQ.front();
        newRoute.push_back(map[curNode][i]);
        routeQ.push(newRoute);
        lengthQ.push(lengthQ.front()+1);
        dontGoThere[map[curNode][i]] = true;
      }
    nodeQ.pop();
    lengthQ.pop();
    routeQ.pop();
  }
  // Can't get through!
  return -1;
}

/* Marks nodes around a certain node w/ a certain range as bad */
void markNodes(int center, vector<bool>& nodeMarks, vector<vector<int> > &map, 
    int range)
{
 // cerr << "markNodes, center=" << center << " range=" << range << endl;
  if (range == 0)
  {
    nodeMarks[center] = true;
    return;
  }
  for (unsigned int i=0; i<map[center].size(); i++)
    markNodes(map[center][i], nodeMarks, map, range-1);
}  

set<int> intersectNodes(set<int> &set1, set<int> &set2)
{
  set<int> retval;
  set<int>::iterator I2;

  for (set<int>::iterator I=set1.begin(); I != set1.end(); ++I) {
    I2 = set2.find(*I);
    if (I2 != set2.end())
      retval.insert(*I);
  }
  return retval;
}

void markNodesSet(int center, set<int> &nodeMarks, vector<vector<int> > &map,
    int range)
{
  if (range == 0)
  {
    nodeMarks.insert(center);
    return;
  }
  for (unsigned int i=0; i<map[center].size(); i++)
    markNodesSet(map[center][i], nodeMarks, map, range-1);
}

/* Look for a path from orig to dest with a set of disallowed nodes. */
bool isNeighbour(int orig, set<int>& targets, World &w, 
        vector<vector<int> > &map)
{
  if (targets.find(orig) != targets.end())
    return true;
  for (unsigned int i=0; i<map[orig].size(); i++)
  {
    if (targets.find(map[orig][i]) != targets.end())
      return true;
  }
  return false;
}