Index: sensor_matrix.c
===================================================================
--- sensor_matrix.c	(revision 667)
+++ sensor_matrix.c	(revision 668)
@@ -1,6 +1,6 @@
 /**
  * Copyright (c) 2007 Colony Project
- * 
+ *
  * Permission is hereby granted, free of charge, to any person
  * obtaining a copy of this software and associated documentation
  * files (the "Software"), to deal in the Software without
@@ -9,10 +9,10 @@
  * copies of the Software, and to permit persons to whom the
  * Software is furnished to do so, subject to the following
  * conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be
  * included in all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
  * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
@@ -41,7 +41,7 @@
 #define DEFAULT_SENSOR_MATRIX_SIZE 20
 
 /*Sensor Matrix Functions*/
-void sensor_matrix_expand(SensorMatrix* m, int nextSize);
+static void sensor_matrix_expand(SensorMatrix* m, int nextSize);
 
 /**
  * Initializes the sensor matrix.
@@ -52,7 +52,7 @@
 {
 	SensorMatrix* m;
 	int i;
-	
+
 	m = (SensorMatrix*)malloc(sizeof(SensorMatrix));
 	if (!m)
 	{
@@ -61,7 +61,18 @@
 	}
 	m->size = DEFAULT_SENSOR_MATRIX_SIZE;
 	m->matrix = (int**)malloc(m->size * sizeof(int*));
+	if (!(m->matrix)) {
+	  WL_DEBUG_PRINT("Out of memory - allocating memory for matrix.\r\n");
+	  free(m);
+	  return NULL;
+	}
 	m->joined = (int*)malloc(m->size * sizeof(int));
+	if (!(m->joined)) {
+	  WL_DEBUG_PRINT("Out of memory - allocating memory for joined.\r\n");
+	  free(m->matrix);
+	  free(m);
+	  return NULL;
+	}
 	m->numJoined = 0;
 	if (!(m->matrix) || !(m->joined))
 	{
@@ -68,7 +79,7 @@
 		WL_DEBUG_PRINT("Out of memory - create sensor matrix 2.\r\n");
 		return NULL;
 	}
-	
+
 	for (i = 0; i < m->size; i++)
 	{
 		m->matrix[i] = NULL;
@@ -95,7 +106,7 @@
 
 /**
  * Adds robot with XBee id id to the sensor matrix.
- * 
+ *
  * @param m the sensor matrix
  * @param id the XBee ID of the robot to add
  **/
@@ -102,11 +113,14 @@
 void sensor_matrix_add_robot(SensorMatrix* m, int id)
 {
 	int i;
-	if (id >= m->size)
+	if (id >= m->size) {
 		sensor_matrix_expand(m, id + 1);
-	if (m->matrix[id] != NULL)
+	}
+
+	if (m->matrix[id] != NULL) {
 		return;
-	
+	}
+
 	m->matrix[id] = (int*)malloc(m->size * sizeof(int));
 	if (!(m->matrix[id]))
 	{
@@ -114,9 +128,11 @@
 		return;
 	}
 
-	for (i = 0; i < m->size; i++)
-		if (m->matrix[i] != NULL)
+	for (i = 0; i < m->size; i++) {
+		if (m->matrix[i] != NULL) {
 			m->matrix[i][id] = -1;
+		}
+	}
 }
 
 /**
@@ -135,14 +151,14 @@
 		WL_DEBUG_PRINT("Removing robot not added to matrix.\r\n");
 		return;
 	}
-	
+
 	free(m->matrix[id]);
 	m->matrix[id] = NULL;
-	
+
 	for (i = 0 ; i < m->size; i++)
 		if (m->matrix[i] != NULL)
 			m->matrix[i][id] = -1;
-	
+
 	m->joined[id] = 0;
 }
 
@@ -154,11 +170,11 @@
  * @param size the new size of the sensor matrix
  **/
 //Note: this has probably not been tested, hopefully it works
-void sensor_matrix_expand(SensorMatrix* m, int nextSize)
+static void sensor_matrix_expand(SensorMatrix* m, int nextSize)
 {
 	int i, j;
 	WL_DEBUG_PRINT("Expanding sensor matrix.\r\n");
-	
+
 	int** tempMatrix = (int**)malloc(nextSize * sizeof(int*));
 	if (!tempMatrix)
 	{
@@ -165,10 +181,10 @@
 		WL_DEBUG_PRINT("Out of memory - expand matrix.\r\n");
 		return;
 	}
-	
+
 	for (i = 0; i < nextSize; i++)
 		tempMatrix[i] = NULL;
-	
+
 	//copy over old sensor data
 	for (i = 0; i < m->size; i++)
 		if (m->matrix[i] != NULL)
@@ -179,14 +195,17 @@
 				WL_DEBUG_PRINT("Out of memory - expand matrix 2.\r\n");
 				return;
 			}
-			for (j = 0; j < m->size; j++)
+			for (j = 0; j < m->size; j++) {
 				tempMatrix[i][j] = m->matrix[i][j];
-			for (j = m->size; j < nextSize; j++)
+			}
+
+			for (j = m->size; j < nextSize; j++) {
 				tempMatrix[i][j] = -1;
-			
+			}
+
 			free(m->matrix[i]);
 		}
-	
+
 	free(m->matrix);
 	m->matrix = tempMatrix;
 	m->size = nextSize;
@@ -198,12 +217,15 @@
 		WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n");
 		return;
 	}
-	
-	for (i = 0; i < m->size; i++)
+
+	for (i = 0; i < m->size; i++) {
 		tempJoined[i] = m->joined[i];
-	for (i = m->size; i < nextSize; i++)
+	}
+
+	for (i = m->size; i < nextSize; i++) {
 		tempJoined[i] = 0;
-	
+	}
+
 	free(m->joined);
 	m->joined = tempJoined;
 }
@@ -210,7 +232,7 @@
 
 /**
  * Sets the sensor reading for robot robot to reading.
- * 
+ *
  * @param m the sensor matrix to set the reading for
  * @param observer the id of the robot who made the reading
  * @param robot the id of the robot who the reading is for
@@ -218,8 +240,10 @@
  */
 void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading)
 {
-	if (robot >= m->size || observer >= m->size || m->matrix[observer] == NULL)
+	if (robot >= m->size || observer >= m->size || m->matrix[observer] == NULL) {
 		sensor_matrix_add_robot(m, observer);
+	}
+
 	m->matrix[observer][robot] = reading;
 }
 
@@ -234,8 +258,10 @@
  **/
 int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot)
 {
-	if (observer >= m->size || robot >= m->size)
+	if (observer >= m->size || robot >= m->size) {
 		return -1;
+	}
+
 	return m->matrix[observer][robot];
 }
 
@@ -248,20 +274,28 @@
  **/
 void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in)
 {
-	if (robot >= m->size)
+	if (robot >= m->size) {
 		sensor_matrix_expand(m, robot + 1);
-	if (in == 1)
+	}
+
+	if (in == 1) {
 		sensor_matrix_add_robot(m, robot);
-	if (in == 1 && m->joined[robot] == 0)
+	}
+
+	if (in == 1 && m->joined[robot] == 0) {
 		m->numJoined++;
-	if (in == 0 && m->joined[robot] == 1)
+	}
+
+	if (in == 0 && m->joined[robot] == 1) {
 		m->numJoined--;
+	}
+
 	m->joined[robot] = in;
 }
 
 /**
  * Checks if the given robot is in the token ring.
- * 
+ *
  * @param m the sensor matrix
  * @param robot the ID of the robot to check
  *
@@ -269,8 +303,10 @@
  **/
 int sensor_matrix_get_in_ring(SensorMatrix* m, int robot)
 {
-	if (robot >= m->size)
+	if (robot >= m->size) {
 		return -1;
+	}
+
 	return m->joined[robot];
 }
 
@@ -278,7 +314,7 @@
  * Returns the size of the sensor matrix.
  *
  * @param m the sensor matrix
- * 
+ *
  * @return the size of the sensor matrix
  **/
 int sensor_matrix_get_size(SensorMatrix* m)
@@ -298,4 +334,3 @@
 {
 	return m->numJoined;
 }
-
