Colony

/**

 * 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

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * 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

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file sensor_matrix.c

 * @brief Sensor Matrix implementation

 *

 * Implementation of a sensor matrix for storing localization implementation.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

#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 = (SensorMatrix*)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 = (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))

        {

                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, int id)

{

        int i;

        if (id >= m->size)

                sensor_matrix_expand(m, id + 1);

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

                return;

        m->matrix[id] = (int*)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, 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 = (int**)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] = (int *)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 = (int *)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;

}