Colony

#include <stdlib.h>

#include <stdio.h>

#include <wl_defs.h>

#include "sensor_matrix.h"

#define DEFAULT_SENSOR_MATRIX_SIZE 20

/*Sensor Matrix Functions*/

void sensor_matrix_expand(SensorMatrix* m, int nextSize);

/**

 * Initializes the sensor matrix.

 *

 * @return the newly created sensor matrix

 **/

SensorMatrix* sensor_matrix_create()

{

        SensorMatrix* m;

        int i;

        m = malloc(sizeof(SensorMatrix));

        if (!m)

        {

                WL_DEBUG_PRINT("Out of memory - create sensor matrix.\r\n");

                return NULL;

        }

        m->size = DEFAULT_SENSOR_MATRIX_SIZE;

        m->matrix = malloc(m->size * sizeof(int*));

        m->joined = malloc(m->size * sizeof(int));

        m->numJoined = 0;

        if (!(m->matrix) || !(m->joined))

        {

                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;

                m->joined[i] = 0;

        }

        return m;

}

/**

 * Deletes and frees memory from the sensor matrix.

 *

 * @param m the sensor matrix to delete

 **/

void sensor_matrix_destroy(SensorMatrix* m)

{

        int i;

        for (i = 0; i < m->size; i++)

                if (m->matrix[i] != NULL)

                        free(m->matrix[i]);

        free(m->matrix);

        free(m->joined);

        free(m);

}

/**

 * 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

 **/

void sensor_matrix_add_robot(SensorMatrix* m, unsigned int id)

{

        int i;

        if (id >= m->size)

                sensor_matrix_expand(m, id + 1);

        if (m->matrix[id] != NULL)

                return;

        m->matrix[id] = malloc(m->size * sizeof(int));

        if (!(m->matrix[id]))

        {

                WL_DEBUG_PRINT("Out of memory - add robot.\r\n");

                return;

        }

        for (i = 0; i < m->size; i++)

                if (m->matrix[i] != NULL)

                        m->matrix[i][id] = -1;

}

/**

 * Removes robot with id from the sensor matrix, and removes

 * all sensor information regarding the robot.

 *

 * @param m the sensor matrix

 * @param id the XBee ID of the robot to remove

 **/

void sensor_matrix_remove_robot(SensorMatrix* m, unsigned int id)

{

        int i;

        if (id >= m->size || m->matrix[id] == NULL)

        {

                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;

}

/**

 * Expands the size of the sensor matrix if an id number we attempt

 * to add is too large.

 *

 * @param m the sensor matrix to expand

 * @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)

{

        int i, j;

        WL_DEBUG_PRINT("Expanding sensor matrix.\r\n");

        int** tempMatrix = malloc(nextSize * sizeof(int*));

        if (!tempMatrix)

        {

                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)

                {

                        tempMatrix[i] = malloc(nextSize * sizeof(int));

                        if (!tempMatrix[i])

                        {

                                WL_DEBUG_PRINT("Out of memory - expand matrix 2.\r\n");

                                return;

                        }

                        for (j = 0; j < m->size; j++)

                                tempMatrix[i][j] = m->matrix[i][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;

        //expand the size of joined

        int* tempJoined = malloc(nextSize * sizeof(int));

        if (!tempJoined)

        {

                WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n");

                return;

        }

        for (i = 0; i < m->size; i++)

                tempJoined[i] = m->joined[i];

        for (i = m->size; i < nextSize; i++)

                tempJoined[i] = 0;

        free(m->joined);

        m->joined = tempJoined;

}

/**

 * 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

 * @param reading the BOM reading from observer to robot

 */

void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading)

{

        if (robot >= m->size || observer >= m->size || m->matrix[observer] == NULL)

                sensor_matrix_add_robot(m, observer);

        m->matrix[observer][robot] = reading;

}

/**

 * Gets the sensor reading for a robot to another robot.

 *

 * @param m the sensor matrix

 * @param observer the robot whose reading we check

 * @param robot the robot who we are checking the reading to

 *

 * @return the observer's BOM reading for robot

 **/

int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot)

{

        if (observer >= m->size || robot >= m->size)

                return -1;

        return m->matrix[observer][robot];

}

/**

 * Sets whether or not the given robot is part of the token ring.

 *

 * @param m the sensor matrix

 * @param robot the robot to set as a member / nonmember of the token ring

 * @param in 1 if the robot is in the token ring, 0 otherwise

 **/

void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in)

{

        if (robot >= m->size)

                sensor_matrix_expand(m, robot + 1);

        if (in == 1)

                sensor_matrix_add_robot(m, robot);

        if (in == 1 && m->joined[robot] == 0)

                m->numJoined++;

        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

 *

 * @return 1 if the robot is in the token ring, 0 otherwise

 **/

int sensor_matrix_get_in_ring(SensorMatrix* m, int robot)

{

        if (robot >= m->size)

                return -1;

        return m->joined[robot];

}

/**

 * 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)

{

        return m->size;

}

/**

 * Returns the number of robots which have joined the

 * token ring.

 *

 * @param m the sensor matrix

 *

 * @return the number of robots in the token ring

 **/

int sensor_matrix_get_joined(SensorMatrix* m)

{

        return m->numJoined;

}