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 queue.c

 * @brief Queue Implementation

 *

 * An implementation of a queue for the colony robots.

 *

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

 **/

#include "queue.h"

#include <wl_defs.h>

#include <stdlib.h>

#include <stdio.h>

#ifndef ROBOT

#include <pthread.h>

#endif

/**

 * @struct node_def

 * A node in the queue.

 **/

typedef struct node_def

{

        /** The item at this point in the queue. **/

        void* val;

        /** The next node in the queue. **/

        struct node_def* next;

} Node;

/**

 * Create a queue.

 *

 * @return the newly created queue.

 **/

Queue* queue_create()

{

        Queue* q = (Queue*)malloc(sizeof(Queue));

        if (q == NULL)

        {

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

                return NULL;

        }

        q->head = NULL;

        q->size = 0;

        #ifndef ROBOT

        pthread_mutex_init(&(q->lock), NULL);

        #endif

        return q;

}

/**

 * Destroys a queue, freeing memory.

 *

 * @param q the queue to destroy

 **/

void queue_destroy(Queue* q)

{

        Node* t = q->head;

        while (t != NULL)

        {

                Node* t2 = t->next;

                free(t);

                t = t2;

        }

        #ifndef ROBOT

        pthread_mutex_destroy(&(q->lock));

        #endif

        free(q);

}

/**

 * Add an element to a queue.

 *

 * @param q the queue to add an element to

 * @param item the item to add to the queue

 **/

int queue_add(Queue* q, void* item)

{

        #ifndef ROBOT

        pthread_mutex_lock(&(q->lock));

        #endif

        Node* n = (Node*)malloc(sizeof(Node));

        if (n == NULL)

        {

                #ifndef ROBOT

                pthread_mutex_unlock(&(q->lock));

                #endif

                return -1;

        }

        n->val = item;

        n->next = NULL;

        if (q->head == NULL)

        {

                q->head = n;

                q->tail = n;

        }

        else

        {

                q->tail->next = n;

                q->tail = n;

        }

        q->size++;

        #ifndef ROBOT

        pthread_mutex_unlock(&(q->lock));

        #endif

        return 0;

}

/**

 * Remove an element from the front of a queue.

 *

 * @param q the queue to remove the element from

 *

 * @return the element which was removed

 **/

void* queue_remove(Queue* q)

{

        #ifndef ROBOT

        pthread_mutex_lock(&(q->lock));

        #endif

        Node* first = q->head;

        if (first == NULL)

        {

                WL_DEBUG_PRINT("Attempting to remove item \

                        from empty queue.\r\n");

                WL_DEBUG_PRINT_INT(queue_size(q));

                #ifndef ROBOT

                pthread_mutex_unlock(&(q->lock));

                #endif

                return NULL;

        }

        void* ret = first->val;

        q->head = first->next;

        if (q->tail == first)

                q->tail = NULL;

        free(first);

        q->size--;

        #ifndef ROBOT

        pthread_mutex_unlock(&(q->lock));

        #endif

        return ret;

}

/**

 * Remove all instances of a given element from a queue.

 *

 * @param q the queue to remove the elements from

 * @param item the element to remove all instances of

 **/

void queue_remove_all(Queue* q, void* item)

{

        #ifndef ROBOT

        pthread_mutex_lock(&(q->lock));

        #endif

        Node* curr = q->head;

        Node* prev = NULL;

        while (curr != NULL)

        {

                Node* next = curr->next;

                if (curr->val == item)

                {

                        if (q->head == curr)

                                q->head = next;

                        if (q->tail == curr)

                                q->tail = prev;

                        if (prev != NULL)

                                prev->next = next;

                        free(curr);

                        q->size--;

                }

                else

                        prev = curr;

                curr = next;

        }

        #ifndef ROBOT

        pthread_mutex_unlock(&(q->lock));

        #endif

}

/**

 * Get the number of elements in the queue.

 *

 * @param q the queue to get the size of

 * @return the size of the queue

 **/

int queue_size(Queue* q)

{

        int size;

        #ifndef ROBOT

        pthread_mutex_lock(&(q->lock));

        #endif

        size = q->size;

        #ifndef ROBOT

        pthread_mutex_unlock(&(q->lock));

        #endif

        return size;

}

/**

 * Check if the queue is empty.

 * 

 * @param q the queue to check

 * @return nonzero if the queue is empty, 0 otherwise

 **/

int queue_is_empty(Queue* q)

{

        int result;

        #ifndef ROBOT

        pthread_mutex_lock(&(q->lock));

        #endif

        result = q->size == 0;

        #ifndef ROBOT

        pthread_mutex_unlock(&(q->lock));

        #endif

        return result;

}