Colony

  map<int, VisionPosition> newPositionMap;

  //TODO: also remove robots that might have disappeared

  int i;

  for (i = 0; i < numPositions; i++) {

      if (isProbablySameRobot(newPos, oldPos)) {

        //TODO: is this the right use of an iterator?

        break;

      }

    }

  for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {

    printf("%d has position (%d, %d)\n", iter->first, iter->second.x, iter->second.y);

  }

    free(positions);

  return 0;

}

int PositionMonitor::assignRealId(int old_id, int real_id) {

  printf("assigning real_id %d to old_id %d\n", real_id, old_id);

  map<int,VisionPosition>::iterator iter = positionMap.find(old_id);

  if (iter == positionMap.end()) {

  positionMap.insert(make_pair(real_id, iter->second));

  positionMap.erase(old_id);

  return 0;

}

map<int, VisionPosition> PositionMonitor::getAllRobotPositions() {

  return positionMap;

}

int PositionMonitor::getRobotPosition(int robot_id, int* xbuf, int* ybuf) {

  //TODO: figure out what a map returns if the element doesn't exist

  if (positionMap.find(robot_id) == positionMap.end()){

    return -1;

  }

  VisionPosition pos = positionMap[robot_id];

  *xbuf = pos.x;

  *ybuf = pos.y;