root / trunk / code / behaviors / bfs_fsm / queue.c @ 708
History | View | Annotate | Download (4.72 KB)
1 | 672 | dsschult | /**
|
---|---|---|---|
2 | * Copyright (c) 2007 Colony Project
|
||
3 | *
|
||
4 | * Permission is hereby granted, free of charge, to any person
|
||
5 | * obtaining a copy of this software and associated documentation
|
||
6 | * files (the "Software"), to deal in the Software without
|
||
7 | * restriction, including without limitation the rights to use,
|
||
8 | * copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
9 | * copies of the Software, and to permit persons to whom the
|
||
10 | * Software is furnished to do so, subject to the following
|
||
11 | * conditions:
|
||
12 | *
|
||
13 | * The above copyright notice and this permission notice shall be
|
||
14 | * included in all copies or substantial portions of the Software.
|
||
15 | *
|
||
16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||
17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||
18 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||
19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||
20 | * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||
21 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||
22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||
23 | * OTHER DEALINGS IN THE SOFTWARE.
|
||
24 | **/
|
||
25 | |||
26 | /**
|
||
27 | * @file queue.c
|
||
28 | * @brief Queue Implementation
|
||
29 | *
|
||
30 | * An implementation of a queue for the colony robots.
|
||
31 | *
|
||
32 | * @author Brian Coltin, Colony Project, CMU Robotics Club
|
||
33 | **/
|
||
34 | |||
35 | #include "queue.h" |
||
36 | #include <wl_defs.h> |
||
37 | |||
38 | #include <stdlib.h> |
||
39 | #include <stdio.h> |
||
40 | #ifndef ROBOT
|
||
41 | #include <pthread.h> |
||
42 | #endif
|
||
43 | |||
44 | /**
|
||
45 | * @struct node_def
|
||
46 | * A node in the queue.
|
||
47 | **/
|
||
48 | typedef struct node_def |
||
49 | { |
||
50 | /** The item at this point in the queue. **/
|
||
51 | void* val;
|
||
52 | /** The next node in the queue. **/
|
||
53 | struct node_def* next;
|
||
54 | } Node; |
||
55 | |||
56 | /**
|
||
57 | * Create a queue.
|
||
58 | *
|
||
59 | * @return the newly created queue.
|
||
60 | **/
|
||
61 | Queue* queue_create() |
||
62 | { |
||
63 | Queue* q = (Queue*)malloc(sizeof(Queue));
|
||
64 | |||
65 | if (q == NULL) |
||
66 | { |
||
67 | WL_DEBUG_PRINT("Out of memory.\r\n");
|
||
68 | return NULL; |
||
69 | } |
||
70 | |||
71 | q->head = NULL;
|
||
72 | q->size = 0;
|
||
73 | |||
74 | #ifndef ROBOT
|
||
75 | pthread_mutex_init(&(q->lock), NULL);
|
||
76 | #endif
|
||
77 | |||
78 | return q;
|
||
79 | } |
||
80 | |||
81 | /**
|
||
82 | * Destroys a queue, freeing memory.
|
||
83 | *
|
||
84 | * @param q the queue to destroy
|
||
85 | **/
|
||
86 | void queue_destroy(Queue* q)
|
||
87 | { |
||
88 | Node* t = q->head; |
||
89 | while (t != NULL) |
||
90 | { |
||
91 | Node* t2 = t->next; |
||
92 | free(t); |
||
93 | t = t2; |
||
94 | } |
||
95 | |||
96 | #ifndef ROBOT
|
||
97 | pthread_mutex_destroy(&(q->lock)); |
||
98 | #endif
|
||
99 | |||
100 | free(q); |
||
101 | } |
||
102 | |||
103 | /**
|
||
104 | * Add an element to a queue.
|
||
105 | *
|
||
106 | * @param q the queue to add an element to
|
||
107 | * @param item the item to add to the queue
|
||
108 | **/
|
||
109 | int queue_add(Queue* q, void* item) |
||
110 | { |
||
111 | #ifndef ROBOT
|
||
112 | pthread_mutex_lock(&(q->lock)); |
||
113 | #endif
|
||
114 | |||
115 | Node* n = (Node*)malloc(sizeof(Node));
|
||
116 | if (n == NULL) |
||
117 | { |
||
118 | #ifndef ROBOT
|
||
119 | pthread_mutex_unlock(&(q->lock)); |
||
120 | #endif
|
||
121 | return -1; |
||
122 | } |
||
123 | |||
124 | n->val = item; |
||
125 | n->next = NULL;
|
||
126 | |||
127 | if (q->head == NULL) |
||
128 | { |
||
129 | q->head = n; |
||
130 | q->tail = n; |
||
131 | } |
||
132 | else
|
||
133 | { |
||
134 | |||
135 | q->tail->next = n; |
||
136 | q->tail = n; |
||
137 | } |
||
138 | |||
139 | q->size++; |
||
140 | |||
141 | #ifndef ROBOT
|
||
142 | pthread_mutex_unlock(&(q->lock)); |
||
143 | #endif
|
||
144 | |||
145 | return 0; |
||
146 | } |
||
147 | |||
148 | /**
|
||
149 | * Remove an element from the front of a queue.
|
||
150 | *
|
||
151 | * @param q the queue to remove the element from
|
||
152 | *
|
||
153 | * @return the element which was removed
|
||
154 | **/
|
||
155 | void* queue_remove(Queue* q)
|
||
156 | { |
||
157 | #ifndef ROBOT
|
||
158 | pthread_mutex_lock(&(q->lock)); |
||
159 | #endif
|
||
160 | |||
161 | Node* first = q->head; |
||
162 | if (first == NULL) |
||
163 | { |
||
164 | WL_DEBUG_PRINT("Attempting to remove item \
|
||
165 | from empty queue.\r\n");
|
||
166 | WL_DEBUG_PRINT_INT(queue_size(q)); |
||
167 | #ifndef ROBOT
|
||
168 | pthread_mutex_unlock(&(q->lock)); |
||
169 | #endif
|
||
170 | return NULL; |
||
171 | } |
||
172 | void* ret = first->val;
|
||
173 | q->head = first->next; |
||
174 | if (q->tail == first)
|
||
175 | q->tail = NULL;
|
||
176 | free(first); |
||
177 | q->size--; |
||
178 | |||
179 | #ifndef ROBOT
|
||
180 | pthread_mutex_unlock(&(q->lock)); |
||
181 | #endif
|
||
182 | |||
183 | return ret;
|
||
184 | } |
||
185 | |||
186 | /**
|
||
187 | * Remove all instances of a given element from a queue.
|
||
188 | *
|
||
189 | * @param q the queue to remove the elements from
|
||
190 | * @param item the element to remove all instances of
|
||
191 | **/
|
||
192 | void queue_remove_all(Queue* q, void* item) |
||
193 | { |
||
194 | #ifndef ROBOT
|
||
195 | pthread_mutex_lock(&(q->lock)); |
||
196 | #endif
|
||
197 | |||
198 | Node* curr = q->head; |
||
199 | Node* prev = NULL;
|
||
200 | |||
201 | while (curr != NULL) |
||
202 | { |
||
203 | Node* next = curr->next; |
||
204 | if (curr->val == item)
|
||
205 | { |
||
206 | if (q->head == curr)
|
||
207 | q->head = next; |
||
208 | if (q->tail == curr)
|
||
209 | q->tail = prev; |
||
210 | if (prev != NULL) |
||
211 | prev->next = next; |
||
212 | free(curr); |
||
213 | q->size--; |
||
214 | } |
||
215 | else
|
||
216 | prev = curr; |
||
217 | curr = next; |
||
218 | } |
||
219 | |||
220 | #ifndef ROBOT
|
||
221 | pthread_mutex_unlock(&(q->lock)); |
||
222 | #endif
|
||
223 | } |
||
224 | |||
225 | /**
|
||
226 | * Get the number of elements in the queue.
|
||
227 | *
|
||
228 | * @param q the queue to get the size of
|
||
229 | * @return the size of the queue
|
||
230 | **/
|
||
231 | int queue_size(Queue* q)
|
||
232 | { |
||
233 | int size;
|
||
234 | |||
235 | #ifndef ROBOT
|
||
236 | pthread_mutex_lock(&(q->lock)); |
||
237 | #endif
|
||
238 | |||
239 | size = q->size; |
||
240 | |||
241 | #ifndef ROBOT
|
||
242 | pthread_mutex_unlock(&(q->lock)); |
||
243 | #endif
|
||
244 | |||
245 | return size;
|
||
246 | } |
||
247 | |||
248 | /**
|
||
249 | * Check if the queue is empty.
|
||
250 | *
|
||
251 | * @param q the queue to check
|
||
252 | * @return nonzero if the queue is empty, 0 otherwise
|
||
253 | **/
|
||
254 | int queue_is_empty(Queue* q)
|
||
255 | { |
||
256 | int result;
|
||
257 | |||
258 | #ifndef ROBOT
|
||
259 | pthread_mutex_lock(&(q->lock)); |
||
260 | #endif
|
||
261 | |||
262 | result = q->size == 0;
|
||
263 | |||
264 | #ifndef ROBOT
|
||
265 | pthread_mutex_unlock(&(q->lock)); |
||
266 | #endif
|
||
267 | |||
268 | return result;
|
||
269 | } |