Fixed typo in queue.h and added simpler testing function