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;

}