Index: wl_token_ring.c
===================================================================
--- wl_token_ring.c	(revision 735)
+++ wl_token_ring.c	(revision 736)
@@ -61,7 +61,8 @@
 #define LEAVING 4
 
 /*Frame Types*/
-#define TOKEN_JOIN_ACCEPT_FRAME 1
+#define TOKEN_JOIN_ACCEPT_FRAME	1
+#define WL_TOKEN_PASS_FRAME	2
 
 /*Function Prototypes*/
 
@@ -77,7 +78,8 @@
 static void wl_token_get_token(void);
 
 /*Packet Handling Routines*/
-static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);
+static void wl_token_pass_receive(int source);
+static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength);
 static void wl_token_bom_on_receive(int source);
 static void wl_token_join_receive(int source);
 static void wl_token_join_accept_receive(int source);
@@ -84,9 +86,6 @@
 
 /*Global Variables*/
 
-//the sensor matrix
-static SensorMatrix* sensorMatrix;
-
 //the robot we are waiting to say it has received the token. -1 if unspecified
 static int wl_token_next_robot = -1;
 
@@ -108,8 +107,10 @@
 // the amount of time a robot has had its BOM on for
 static int bom_on_count = 0;
 
+#ifndef ROBOT
 static void do_nothing(void) {}
 static int get_nothing(void) {return -1;}
+#endif
 
 #ifdef ROBOT
 #ifndef FIREFLY
@@ -178,9 +179,9 @@
 		return -1;
 	}
 
-	sensorMatrix = sensor_matrix_create();
+	sensor_matrix_create();
 	//add ourselves to the sensor matrix
-	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
+	sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
 
 	wl_register_packet_group(&wl_token_ring_handler);
 
@@ -224,7 +225,6 @@
 // it reduces code size or not should be done to be sure.
 static void wl_token_ring_cleanup()
 {
-	sensor_matrix_destroy(sensorMatrix);
 }
 
 /**
@@ -239,7 +239,7 @@
 		//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);
+			sensor_matrix_set_in_ring(wl_token_next_robot, 0);
 			WL_DEBUG_PRINT("Robot ");
 			WL_DEBUG_PRINT_INT(wl_token_next_robot);
 			WL_DEBUG_PRINT(" has died.\r\n");
@@ -256,10 +256,10 @@
 	//we must start our own token ring, no one is responding to us
 	if (joinDelay == 0)
 	{
-		if (sensor_matrix_get_joined(sensorMatrix) == 0)
+		if (sensor_matrix_get_joined() == 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);
+			sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
 			ringState = MEMBER;
 			//this will make us pass the token to ourself
 			//repeatedly, and other robots when they join
@@ -314,13 +314,11 @@
 	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);
+			wl_token_pass_receive(source);
 			break;
+		case WL_TOKEN_SENSOR_MATRIX:
+			wl_token_sensor_matrix_receive(source, packet, length);
+			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);
@@ -350,7 +348,7 @@
 {
 	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 sensor_matrix_get_reading(source, dest);
 	}
 
 	return -1;
@@ -381,7 +379,7 @@
 // it reduces code size or not should be done to be sure.
 int wl_token_get_robots_in_ring(void)
 {
-	return sensor_matrix_get_joined(sensorMatrix);
+	return sensor_matrix_get_joined();
 }
 
 /**
@@ -395,7 +393,7 @@
 // it reduces code size or not should be done to be sure.
 int wl_token_is_robot_in_ring(int robot)
 {
-	return sensor_matrix_get_in_ring(sensorMatrix, robot);
+	return sensor_matrix_get_in_ring(robot);
 }
 
 /**
@@ -410,12 +408,12 @@
   // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
   // the overhead of a function call each iteration, call it only once before the loop and store
   // the value in a variable and check against that variable in the loop condition
-	while (!sensor_matrix_get_in_ring(sensorMatrix, i) && i < sensor_matrix_get_size(sensorMatrix)) {
+	while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
 		i++;
 	}
 
   //TODO: if you do the above comment, then you can compare this to the variable also
-	if (i == sensor_matrix_get_size(sensorMatrix)) {
+	if (i == sensor_matrix_get_size()) {
 		i = -1;
 	}
 
@@ -457,13 +455,13 @@
   // the overhead of a function call each iteration, call it only once before the loop and store
   // the value in a variable and check against that variable in the loop condition
 	iteratorCount++;
-	while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount)
-		&& iteratorCount < sensor_matrix_get_size(sensorMatrix)) {
+	while (!sensor_matrix_get_in_ring(iteratorCount)
+		&& iteratorCount < sensor_matrix_get_size()) {
 		iteratorCount++;
 	}
 
   //TODO: if you do the above comment, then you can compare this to the variable also
-	if (iteratorCount == sensor_matrix_get_size(sensorMatrix)) {
+	if (iteratorCount == sensor_matrix_get_size()) {
 		iteratorCount = -1;
 	}
 
@@ -479,7 +477,7 @@
 // it reduces code size or not should be done to be sure.
 int wl_token_get_num_robots(void)
 {
-	return sensor_matrix_get_joined(sensorMatrix);
+	return sensor_matrix_get_joined();
 }
 
 /**
@@ -491,11 +489,37 @@
 // it reduces code size or not should be done to be sure.
 int wl_token_get_matrix_size(void)
 {
-	return sensor_matrix_get_size(sensorMatrix);
+	return sensor_matrix_get_size();
 }
 
 /**
  * This method is called when we receive a token pass packet.
+ *
+ * @param source the robot who passed the token to us.
+ **/
+static void wl_token_pass_receive(int source)
+{
+	WL_DEBUG_PRINT("Received token from ");
+	WL_DEBUG_PRINT_INT(source);
+	WL_DEBUG_PRINT(", expected ");
+	WL_DEBUG_PRINT_INT(wl_token_next_robot);
+	WL_DEBUG_PRINT(".\n");
+	// 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 && wl_get_xbee_id() != 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;
+	sensor_matrix_set_in_ring(source, 1);
+	wl_token_get_token();
+}
+
+/**
+ * This method is called when we receive a token pass packet.
  * @param source is the robot it came from
  * @param nextRobot is the robot the token was passed to
  * @param sensorData a char with an id followed by a char with the sensor
@@ -502,27 +526,14 @@
  *		reading for that robot, repeated for sensorDataLength bytes
  * @param sensorDataLength the length in bytes of sensorData
  */
-static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
+static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
 {
 	int i, j;
+	char nextRobot;
 
-	// 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);
+	sensor_matrix_set_in_ring(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)
@@ -531,7 +542,7 @@
   // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
   // the overhead of a function call each iteration, call it only once before the loop and store
   // the value in a variable and check against that variable in the loop condition
-	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
+	for (i = 0; i < sensor_matrix_get_size(); i++)
 	{
 		if (i == source) {
 			continue;
@@ -546,14 +557,24 @@
 				accepted = -1;
 				WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
 			}
-			sensor_matrix_set_reading(sensorMatrix, source, i,
+			sensor_matrix_set_reading(source, i,
 						sensorData[2 * j + 1]);
-			sensor_matrix_set_in_ring(sensorMatrix, i, 1);
+			if (!sensor_matrix_get_in_ring(i))
+			{
+				WL_DEBUG_PRINT("Robot ");
+				WL_DEBUG_PRINT_INT(i);
+				WL_DEBUG_PRINT(" has been added to 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(i, 1);
 			j++;
 		}
 		else
 		{
-			if (sensor_matrix_get_in_ring(sensorMatrix, i))
+			if (sensor_matrix_get_in_ring(i))
 			{
 				WL_DEBUG_PRINT("Robot ");
 				WL_DEBUG_PRINT_INT(i);
@@ -562,7 +583,7 @@
 				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);
+				sensor_matrix_set_in_ring(i, 0);
 			}
 
 			if (i == wl_get_xbee_id() && ringState == MEMBER)
@@ -577,8 +598,7 @@
 			//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);
+				sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
 				ringState = NONMEMBER;
 				acceptor = -1;
 				wl_token_ring_join();
@@ -585,15 +605,31 @@
 			}
 		}
 	}
+	
+	// get the next robot in the token ring
+	i = source + 1;
+	while (1)
+	{
+		if (i == sensor_matrix_get_size()) {
+			i = 0;
+		}
 
-	wl_token_next_robot = nextRobot;
+		if (sensor_matrix_get_in_ring(i) || i == source)
+		{
+			nextRobot = (char)i;
+			break;
+		}
 
+		i++;
+	}
+
+	if (nextRobot != wl_get_xbee_id())
+		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();
-	}
+	if (sensor_matrix_get_joined() == 0 && ringState == JOINING)
+		wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME);
 }
 
 /**
@@ -615,11 +651,11 @@
     // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
     // the overhead of a function call each iteration, call it only once before the loop and store
     // the value in a variable and check against that variable in the loop condition
-		if (curr == sensor_matrix_get_size(sensorMatrix))
+		if (curr == sensor_matrix_get_size())
 			curr = 0;
 		if (curr == robot2)
 			break;
-		if (sensor_matrix_get_in_ring(sensorMatrix, curr))
+		if (sensor_matrix_get_in_ring(curr))
 			count++;
 		curr++;
 	}
@@ -631,8 +667,9 @@
  **/
 static int wl_token_pass_token()
 {
-	char nextRobot;
+	char nextRobot = 0xFF;
 	int i = wl_get_xbee_id() + 1;
+	char buf[2 * sensor_matrix_get_size()];
 	if (accepted == -1)
 	{
 		while (1)
@@ -641,11 +678,11 @@
       // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
       // the overhead of a function call each iteration, call it only once before the loop and store
       // the value in a variable and check against that variable in the loop condition
-			if (i == sensor_matrix_get_size(sensorMatrix)) {
+			if (i == sensor_matrix_get_size()) {
 				i = 0;
 			}
 
-			if (sensor_matrix_get_in_ring(sensorMatrix, i))
+			if (sensor_matrix_get_in_ring(i))
 			{
 				nextRobot = (char)i;
 				break;
@@ -658,48 +695,36 @@
 	{
 		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);
+		sensor_matrix_set_in_ring(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");
-		free(buf);
-		return -1;
-	}
-	buf[0] = nextRobot;
-
 	int j = 0;
   //TODO: the compiler may or may not optimize this such that my comment is useless:
   // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
   // the overhead of a function call each iteration, call it only once before the loop and store
   // the value in a variable and check against that variable in the loop condition
-	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) {
-		if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
+	for (i = 0; i < sensor_matrix_get_size(); i++) {
+		if (sensor_matrix_get_in_ring(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);
+			buf[2*j] = i;
+			buf[2*j + 1] = sensor_matrix_get_reading(wl_get_xbee_id(), i);
 			j++;
 		}
 	}
 
+	int packetSize = 2 * j * sizeof(char);
 	WL_DEBUG_PRINT("Passing the token to robot ");
-	WL_DEBUG_PRINT_INT(buf[0]);
+	WL_DEBUG_PRINT_INT(nextRobot);
 	WL_DEBUG_PRINT(".\r\n");
-	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, buf, packetSize, 3) != 0) {
-		free(buf);
+	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
 		return -1;
-	}
+	if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
+		return -1;
 
 	wl_token_next_robot = nextRobot;
 	deathDelay = DEATH_DELAY;
-	free(buf);
 
 	return 0;
 }
@@ -719,7 +744,7 @@
 
 	bom_on_count = 0;
 
-	sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(),
+	sensor_matrix_set_reading(wl_get_xbee_id(),
 		source, get_max_bom_function());
 }
 
@@ -735,7 +760,7 @@
 	WL_DEBUG_PRINT("We have the token.\r\n");
 	if (ringState == ACCEPTED)
 	{
-		sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
+		sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
 		WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
 		ringState = MEMBER;
 		joinDelay = -1;
@@ -743,7 +768,7 @@
 
 	if (ringState == LEAVING || ringState == NONMEMBER)
 	{
-		sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
+		sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
 		if (ringState == NONMEMBER)
 		{
 			WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
@@ -760,7 +785,7 @@
 	#endif
 	bom_off_function();
 
-	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
+	if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
 	{
 		WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
 		return;
@@ -798,7 +823,7 @@
 	while (1)
 	{
 		if (i < 0)
-			i = sensor_matrix_get_size(sensorMatrix) - 1;
+			i = sensor_matrix_get_size() - 1;
 
 		//we must send a join acceptance
 		if (i == wl_get_xbee_id())
@@ -805,7 +830,7 @@
 			break;
 
 		//another robot will handle it
-		if (sensor_matrix_get_in_ring(sensorMatrix, i))
+		if (sensor_matrix_get_in_ring(i))
 			return;
 
 		i--;
@@ -820,7 +845,7 @@
 	WL_DEBUG_PRINT(" into the token ring.\r\n");
 
 	// the token ring has not started yet
-	if (sensor_matrix_get_joined(sensorMatrix) == 1)
+	if (sensor_matrix_get_joined() == 1)
 		wl_token_pass_token();
 }
 
@@ -841,5 +866,5 @@
 	acceptor = source;
 
 	//add ourselves to the token ring
-	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
+	sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
 }
