Colony

				   int length);

  {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,

   wl_token_ring_response_handler, wl_token_ring_receive_handler,

   wl_token_ring_cleanup};

void wl_token_ring_register() {

  if (wl_get_xbee_id() > 0xFF) {

    //Note: if this becomes an issue (unlikely), we could limit sensor information

    //to half a byte and use 12 bits for the id

    WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");

    WL_DEBUG_PRINT_INT(wl_get_xbee_id());

    WL_DEBUG_PRINT(".\r\n");

    return;

  }

  sensorMatrix = sensor_matrix_create();

  //add ourselves to the sensor matrix

  sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);

  wl_register_packet_group(&wl_token_ring_handler);

void wl_token_ring_unregister() {

  wl_unregister_packet_group(&wl_token_ring_handler);

				     void (*off_function) (void), int (*max_bom_function) (void)) {

  bom_on_function = on_function;

  bom_off_function = off_function;

  get_max_bom_function = max_bom_function;

void wl_token_ring_cleanup() {

  sensor_matrix_destroy(sensorMatrix);

void wl_token_ring_timeout_handler() {

  //someone is not responding, assume they are dead

  if (deathDelay == 0) {

    //pass the token to the next robot if we think someone has died

    //also, declare that person dead, as long as it isn't us

    if (wl_token_next_robot != wl_get_xbee_id()) {

      sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);

      WL_DEBUG_PRINT("Robot ");

      WL_DEBUG_PRINT_INT(wl_token_next_robot);

      WL_DEBUG_PRINT(" has died.\r\n");

      wl_token_next_robot = -1;

      deathDelay = DEATH_DELAY;

    }

    // we may have been dropped from the ring when this is received

    if (ringState == MEMBER)

      wl_token_pass_token();

  }

  //we must start our own token ring, no one is responding to us

  if (joinDelay == 0) {

    if (sensor_matrix_get_joined(sensorMatrix) == 0) {

      WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");

      sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);

      ringState = MEMBER;

      //this will make us pass the token to ourself

      //repeatedly, and other robots when they join

      deathDelay = DEATH_DELAY;

      wl_token_next_robot = wl_get_xbee_id();

    } else {

      WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");

      //attempt to rejoin with a random delay

      wl_token_ring_join();

      joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;

    }

  }

  if (deathDelay >= 0)

    deathDelay--;

  if (joinDelay >= 0)

    joinDelay--;

  if (bom_on_count >= 0)

    bom_on_count++;

void wl_token_ring_response_handler(int frame, int received) {

  if (!received) {

    WL_DEBUG_PRINT("FAILED.\r\n");

  }

				   int length) {

  switch (type) {

  case WL_TOKEN_PASS:

    if (length < 1) {

      WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");

      return;

    }

    wl_token_pass_receive(source, packet[0], packet + 1, length - 1);

    break;

  case WL_TOKEN_BOM_ON:

    //add the robot to the sensor matrix if it is not already there

    wl_token_bom_on_receive(source);

    break;

  case WL_TOKEN_JOIN:

    wl_token_join_receive(source);

    break;

  case WL_TOKEN_JOIN_ACCEPT:

    wl_token_join_accept_receive(source);

    break;

  default:

    WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");

    break;

  }

void wl_token_ring_join() {

  WL_DEBUG_PRINT("Joining the token ring.\r\n");

  ringState = JOINING;

  joinDelay = DEATH_DELAY * 2;

  wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,

			NULL, 0, 0);

void wl_token_ring_leave() {

  ringState = LEAVING;

int wl_token_get_sensor_reading(int source, int dest) {

  if (wl_token_is_robot_in_ring(dest) &&

      (source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source)))

    return sensor_matrix_get_reading(sensorMatrix, source, dest);

  return -1;

int wl_token_get_my_sensor_reading(int dest) {

  return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);

void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength) {

  int i, j;

  // this prevents two tokens from being passed around at a time (second clause is in case we are joining)

  if (source != wl_token_next_robot && bom_on_count <= DEATH_DELAY / 2 &&

      ringState != ACCEPTED) {

    WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");

    WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");

    return;

  }

  bom_on_count = -1;

  deathDelay = -1;

  WL_DEBUG_PRINT("Received the token from robot");

  WL_DEBUG_PRINT_INT(source);

  WL_DEBUG_PRINT(", next robot is ");

  WL_DEBUG_PRINT_INT((int)nextRobot);

  WL_DEBUG_PRINT(" \r\n");

  sensor_matrix_set_in_ring(sensorMatrix, source, 1);

  //with this packet, we are passed the id of the next robot in the ring

  //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)

  j = 0;

  for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) {

    if (i == source)

      continue;

    //set the sensor information we receive

    if (j < sensorDataLength / 2 && sensorData[2 * j] == i) {

      //the robot we were going to accept has already been accepted

      if (accepted == i) {

	accepted = -1;

	WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");

      }

      sensor_matrix_set_reading(sensorMatrix, source, i,

				sensorData[2 * j + 1]);

      sensor_matrix_set_in_ring(sensorMatrix, i, 1);

      j++;

    } else {

      if (sensor_matrix_get_in_ring(sensorMatrix, i)) {

	WL_DEBUG_PRINT("Robot ");

	WL_DEBUG_PRINT_INT(i);

	WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");

	WL_DEBUG_PRINT_INT(wl_get_xbee_id());

	WL_DEBUG_PRINT(" due to a packet from robot ");

	WL_DEBUG_PRINT_INT(source);

	WL_DEBUG_PRINT(".\r\n");

	sensor_matrix_set_in_ring(sensorMatrix, i, 0);

      }

      if (i == wl_get_xbee_id() && ringState == MEMBER) {

	ringState = NONMEMBER;

	wl_token_ring_join();

	WL_DEBUG_PRINT("We have been removed from the ring ");

	WL_DEBUG_PRINT("and are rejoining.\r\n");

      }

      //the person who accepted us is dead... let's ask again

      if (i == acceptor) {

	sensor_matrix_set_in_ring(sensorMatrix,

				  wl_get_xbee_id(), 1);

	ringState = NONMEMBER;

	acceptor = -1;

	wl_token_ring_join();

      }

    }

  }

  wl_token_next_robot = nextRobot;

  deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;

  //we have the token

  if (wl_token_next_robot == wl_get_xbee_id())

    wl_token_get_token();

int get_token_distance(int robot1, int robot2) {

  int curr = robot1 + 1;

  int count = 1;

  while (1) {

    if (curr == sensor_matrix_get_size(sensorMatrix))

      curr = 0;

    if (curr == robot2)

      break;

    if (sensor_matrix_get_in_ring(sensorMatrix, curr))

      count++;

    curr++;

  }

  return count;

void wl_token_pass_token() {

  char nextRobot;

  int i = wl_get_xbee_id() + 1;

  if (accepted == -1) {

    while (1) {

      if (i == sensor_matrix_get_size(sensorMatrix))

	i = 0;

      if (sensor_matrix_get_in_ring(sensorMatrix, i)) {

	nextRobot = (char)i;

	break;

      }

      i++;

    }

  } else {

    WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");

    //add a new robot to the token ring

    sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);

    nextRobot = accepted;

    accepted = -1;

  }

  //we don't include ourself

  int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);

  char* buf = (char*)malloc(packetSize * sizeof(char));

  if (!buf) {

    WL_DEBUG_PRINT_INT(packetSize);

    WL_DEBUG_PRINT("Out of memory - pass token.\r\n");

    return;

  }

  buf[0] = nextRobot;

  int j = 0;

  for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)

    if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id()) {

      buf[2*j + 1] = i;

      buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);

      j++;

    }

  WL_DEBUG_PRINT("Passing the token to robot ");

  WL_DEBUG_PRINT_INT(buf[0]);

  WL_DEBUG_PRINT(".\r\n");

  wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,

			buf, packetSize, 3);

  wl_token_next_robot = nextRobot;

  deathDelay = DEATH_DELAY;

  free(buf);

void wl_token_bom_on_receive(int source) {

  WL_DEBUG_PRINT("Robot ");

  WL_DEBUG_PRINT_INT(source);

  WL_DEBUG_PRINT(" has flashed its bom.\r\n");

  bom_on_count = 0;

  sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), 

			    source, get_max_bom_function());

void wl_token_get_token() {

  WL_DEBUG_PRINT("We have the token.\r\n");

  if (ringState == ACCEPTED) {

    sensor_matrix_set_in_ring(sensorMatrix,

			      wl_get_xbee_id(), 1);

    WL_DEBUG_PRINT("Now a member of the token ring.\r\n");

    ringState = MEMBER;

    joinDelay = -1;

  }

  if (ringState == LEAVING || ringState == NONMEMBER) {

    sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);

    if (ringState == NONMEMBER) {

      WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");

    }

    return;

  }

  WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");

  wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,

			NULL, 0, 0);

  bom_on_function();

#ifdef ROBOT

  delay_ms(BOM_DELAY);

#endif

  bom_off_function();

  if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id())) {

    WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");

    return;

  }

  wl_token_pass_token();

void wl_token_join_receive(int source) {

  WL_DEBUG_PRINT("Received joining request from robot ");

  WL_DEBUG_PRINT_INT(source);

  WL_DEBUG_PRINT(".\r\n");

  //we cannot accept the request if we are not a member

  if (ringState != MEMBER)

    return;

  //if they didn't get our response, see if we should respond again

  if (accepted == source)

    accepted = -1;

  //we can only accept one request at a time

  if (accepted != -1)

    return;

  //check if we are the preceding robot in the token ring

  int i = source - 1;

  while (1) {

    if (i < 0)

      i = sensor_matrix_get_size(sensorMatrix) - 1;

    //we must send a join acceptance

    if (i == wl_get_xbee_id())

      break;

    //another robot will handle it

    if (sensor_matrix_get_in_ring(sensorMatrix, i))

      return;

    i--;

  }

  accepted = source;

  wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,

				       NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);

  WL_DEBUG_PRINT("Accepting robot ");

  WL_DEBUG_PRINT_INT(source);

  WL_DEBUG_PRINT(" into the token ring.\r\n");

  // the token ring has not started yet

  if (sensor_matrix_get_joined(sensorMatrix) == 1)

    wl_token_pass_token();

void wl_token_join_accept_receive(int source) {

  WL_DEBUG_PRINT("Accepted into the token ring by robot ");

  WL_DEBUG_PRINT_INT(source);

  WL_DEBUG_PRINT(".\r\n");

  joinDelay = JOIN_DELAY;

  ringState = ACCEPTED;

  acceptor = source;

  //add ourselves to the token ring

  sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);

int wl_token_get_robots_in_ring(void) {

  return sensor_matrix_get_joined(sensorMatrix);

int wl_token_is_robot_in_ring(int robot) {

  return sensor_matrix_get_in_ring(sensorMatrix, robot);

void wl_token_iterator_begin(void) {

  int i = 0;

  iteratorCount = 0;

  while (!sensor_matrix_get_in_ring(sensorMatrix, i) &&

	 i < sensor_matrix_get_size(sensorMatrix))

    i++;

  if (i == sensor_matrix_get_size(sensorMatrix))

    i = -1;

  iteratorCount = i;

int wl_token_iterator_has_next(void) {

  return iteratorCount != -1;

int wl_token_iterator_next(void) {

  int result = iteratorCount;

  if (result < 0)

    return result;

  iteratorCount++;

  while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount) &&

	 iteratorCount < sensor_matrix_get_size(sensorMatrix))

    iteratorCount++;

  if (iteratorCount == sensor_matrix_get_size(sensorMatrix))

    iteratorCount = -1;

  return result;

int wl_token_get_num_robots(void) {

  return sensor_matrix_get_joined(sensorMatrix);

int wl_token_get_matrix_size(void) {

  return sensor_matrix_get_size(sensorMatrix);

typedef struct {

  /**

   * The size of the sensor matrix.

   **/

  int size;

  /**

   * The matrix. Each row represents the readings of one

   * robot.

   **/

  int** matrix;

  /**

   * The element representing a robot is true if that robot

   * is in the token ring and false otherwise.

   **/

  int* joined;

  /**

   * The number of robots in the token ring.

   **/

  int numJoined;

typedef struct {

				     char* extra, int extraLen);

ISR(USART1_RX_vect) {

  char c = UDR1;

  arrival_buf[buffer_last] = c;

  int t = buffer_last + 1;

  if (t == XBEE_BUFFER_SIZE)

    t = 0;

  if (t == buffer_first) {

    WL_DEBUG_PRINT("Out of space in buffer.\n");

  }

  buffer_last = t;

SIGNAL(SIG_USART0_RECV) {

  char c = UDR0;

  arrival_buf[buffer_last] = c;

  int t = buffer_last + 1;

  if (t == XBEE_BUFFER_SIZE)

    t = 0;

  if (t == buffer_first) {

    WL_DEBUG_PRINT("Out of space in buffer.\n");

  }

  buffer_last = t;

void* listen_to_xbee(void* x) {

  char c;

  while (1) {

    xbee_read(&c, 1);

    arrival_buf[buffer_last] = c;

    int t = buffer_last + 1;

    if (t == XBEE_BUFFER_SIZE)

      t = 0;

    if (t == buffer_first) {

      WL_DEBUG_PRINT("Out of space in buffer.\n");

    }

    buffer_last = t;

  }

  return 0;

int xbee_lib_init(void) {

  arrival_buf[0] = 'A';

  arrival_buf[1] = 'A';

  arrival_buf[2] = 'A';

#ifdef ROBOT

  //enable the receiving interrupt

#ifdef FIREFLY

  UCSR0B |= _BV(RXCIE) | _BV(RXEN);

#else

  UCSR1B |= _BV(RXCIE);

#endif

  sei();

#else

  printf("Connecting to port %s.\n", xbee_com_port);

  xbee_stream = open(xbee_com_port, O_RDWR);

  if (xbee_stream == -1/* || lockf(xbee_stream, F_TEST, 0) != 0*/) {

    printf("Failed to open connection to XBee on port %s\r\n", xbee_com_port);

    return -1;

  }

  // set baud rate, etc. correctly

  struct termios options;

  tcgetattr(xbee_stream, &options);

  cfsetispeed(&options, B9600);

  cfsetospeed(&options, B9600);

  options.c_iflag &= ~ICRNL;

  options.c_oflag &= ~OCRNL;

  options.c_cflag |= (CLOCAL | CREAD);

  options.c_cflag &= ~PARENB;

  options.c_cflag &= ~CSTOPB;

  options.c_cflag &= ~CSIZE;

  options.c_cflag |= CS8;

  options.c_lflag &= ~ICANON;

  options.c_cc[VMIN] = 1;

  options.c_cc[VTIME] = 50;

  if (tcsetattr(xbee_stream, TCSANOW, &options)) {

    fprintf(stderr, "Error setting attributes.\n");

    return -1;

  }

  //lockf(xbee_stream, F_LOCK, 0);

  xbee_listen_thread =

    (pthread_t*)malloc(sizeof(pthread_t));

  if (xbee_listen_thread == NULL) {

    fprintf(stderr, "%s: Malloc failed.\n", __FUNCTION__);

    return -1;

  }

  int ret = pthread_create(xbee_listen_thread, NULL,

			   listen_to_xbee, NULL);

  if (ret) {

    fprintf(stderr, "Failed to create listener thread.\r\n");

    return -1;

  }

#endif

  xbee_enter_command_mode();

  xbee_enter_api_mode();

  xbee_exit_command_mode();

  xbee_send_read_at_command("MY");

  //wait to return until the address is set

  while (xbee_address == 0) xbee_get_packet(NULL);

  return 0;

void xbee_terminate() {

#ifndef ROBOT

  pthread_cancel(*xbee_listen_thread);

  free(xbee_listen_thread);

  lockf(xbee_stream, F_ULOCK, 0);

  close(xbee_stream);

#endif

void xbee_send(char* buf, int size) {

#ifdef ROBOT

  int i;

  for (i = 0; i < size; i++)

    xbee_putc(buf[i]);

#else

  int ret = write(xbee_stream, buf, size);

  //success

  if (ret == size)

    return;

  if (ret == -1) {

    //interrupted by system signal, probably timer interrupt.

    //just try again

    if (errno == 4) {

      xbee_send(buf, size);

      return;

    }

    printf("Failed to write to xbee, error %i.\r\n", errno);

    return;

  }

  //write was interrupted after writing ret bytes

  xbee_send(buf + ret, size - ret);

#endif

void xbee_send_string(char* c) {

  xbee_send(c, strlen(c));

void xbee_read(char* buf, int size) {

  if (read(xbee_stream, buf, size) == -1)

    printf("Failed to read from xbee.\r\n");

void xbee_enter_command_mode() {

  xbee_send_string("+++");

  xbee_wait_for_ok();

void xbee_exit_command_mode() {

  xbee_send_string("ATCN\r");

  xbee_wait_for_ok();

void xbee_enter_api_mode() {

  xbee_send_string("ATAP 1\r");

  xbee_wait_for_ok();

void xbee_exit_api_mode() {

  xbee_send_string("ATAP 0\r");

void xbee_wait_for_ok() {

  xbee_wait_for_string("OK\r", 3);

void xbee_wait_for_string(char* s, int len) {

  char* curr = s;

  while (curr - s < len) {

    // check if buffer is empty

    if (buffer_last == buffer_first)

      continue;

    char c = arrival_buf[buffer_first++];

    if (buffer_first == XBEE_BUFFER_SIZE)

      buffer_first = 0;

    if (c == *curr)

      curr++;

    else

      curr = s;

  }

int xbee_verify_checksum(char* packet, int len) {

  unsigned char sum = 0;

  int i;

  for (i = 3; i < len; i++)

    sum += (unsigned char)packet[i];

  return sum == 0xFF;

char xbee_compute_checksum(char* buf, int len) {

  int i;

  unsigned char sum = 0;

  for (i = 0; i < len; i++)

    sum += (unsigned char)buf[i];

  return 0xFF - sum;