diff --git a/examples/serial_listener_example.cc b/examples/serial_listener_example.cc
index 83ef3c1..deb0646 100644
--- a/examples/serial_listener_example.cc
+++ b/examples/serial_listener_example.cc
@@ -13,21 +13,16 @@ void callback(std::string line) {
   std::cout << "callback got a: " << line << std::endl;
 }
 
-bool comparator(std::string line) {
-  if (line.substr(0,2) == "V=")
-    return true;
-  return false;
-}
-
+#if 0
 int main(void) {
   Serial serial("/dev/tty.usbmodemfd1231", 115200);
 
   SerialListener listener;
-  // Set the time to live for messages to 1 second
-  listener.setTimeToLive(1000);
-  listener.startListening(&serial);
+  // Set the time to live for messages to 10 milliseconds
+  listener.setTimeToLive(10);
+  listener.startListening(serial);
 
-  listener.listenFor(comparator, callback);
+  listener.listenFor(SerialListener::startsWith("V="), callback);
 
   serial.write("?$1E\r");
   if (!listener.listenForStringOnce("?$1E")) {
@@ -35,12 +30,32 @@ int main(void) {
     return 1;
   }
 
+  serial.write("?V\r");
+  serial.write("# 1\r");
+
+  while (true) {
+    // Sleep 100 ms
+    SerialListener::sleep(100);
+  }
+
 }
+#endif
 
-/*
-TODO:
+int main(void) {
+  Serial serial("/dev/tty.usbmodemfd1231", 115200);
 
-listenForOnce -> listenForStringOnce
-listenForOnce(ComparatorType comparator, std::string& result, size_t timeout)
+  serial.write("?$1E\r");
+  SerialListener::sleep(10);
+  // if ("?$1E\r" != serial.read(5)) {
+  //   std::cerr << "Didn't get conformation of device version!" << std::endl;
+  //   return 1;
+  // }
 
-*/
\ No newline at end of file
+  serial.write("?V\r");
+  serial.write("# 1\r");
+
+  while (true) {
+    std::cout << serial.read(5) << std::endl;
+  }
+
+}
diff --git a/include/serial/serial_listener.h b/include/serial/serial_listener.h
index e6ee241..2204849 100644
--- a/include/serial/serial_listener.h
+++ b/include/serial/serial_listener.h
@@ -280,7 +280,7 @@ public:
    * \param serial_port Pointer to a serial::Serial object that is used to 
    * retrieve new data.
    */
-  void startListening (serial::Serial * serial_port);
+  void startListening (serial::Serial &serial_port);
 
   /*!
    * Stops the listening thread and blocks until it completely stops.
@@ -306,6 +306,20 @@ public:
    */
   bool listenForStringOnce (std::string token, size_t timeout = 1000);
 
+  /*!
+   * Blocks until the comparator returns true or until the timeout occurs.
+   * 
+   * \param comparator ComparatorType function that should return true if the
+   * given std::string matches otherwise false.
+   * 
+   * \param timeout in milliseconds before timing out and returning false.
+   * Defaults to 1000 milliseconds or 1 second.
+   * 
+   * \return bool If true then the token was detected before the token, false 
+   * if the token was not heard and the timeout occured.
+   */
+  bool listenForOnce (ComparatorType comparator, size_t timeout = 1000);
+
   /*!
    * Setups up a filter that calls a callback when a comparator returns true.
    * 
@@ -447,6 +461,16 @@ public:
 
 /***** Static Functions ******/
 
+  /*!
+   * Sleeps for a given number of milliseconds.
+   * 
+   * \param ms number of milliseconds to sleep.
+   */
+  static void
+  sleep (size_t ms) {
+    boost::this_thread::sleep(boost::posix_time::milliseconds(ms));
+  }
+
   /*!
    * This returns a tokenizer that splits on a given delimeter.
    * 
@@ -455,28 +479,155 @@ public:
    * 
    * Example:
    * <pre>
-   *   my_listener.setTokenizer(delimeter_tokenizer("\r"));
+   *   my_listener.setTokenizer(SerialListener::delimeter_tokenizer("\r"));
    * <\pre>
    * 
+   * \param delimeter A std::string that is used as a delimeter when 
+   * tokenizing data.
+   * 
+   * \return TokenizerType A tokenizer function type that can be passed to 
+   * SerialListener::setTokenizer.
+   * 
    * \see SerialListener::setTokenizer, serial::TokenizerType
    */
   static TokenizerType
   delimeter_tokenizer (std::string delimeter);
 
-  // tokenizer functions
+  // delimeter tokenizer function
   static void
   _delimeter_tokenizer (std::string &data, std::vector<std::string> &tokens,
                         std::string delimeter);
 
+  /*!
+   * This returns a comparator that matches only the exact string given.
+   * 
+   * This can be used with listenFor or listenForOnce:
+   * 
+   * Example:
+   * <pre>
+   *   my_listener.listenFor(SerialListener::exactly("my_string"),
+   *                         my_callback);
+   * <\pre>
+   * 
+   * \param exact_str A std::string that is used as the exact string to match 
+   * when comparing tokens for matching.
+   * 
+   * \return ComparatorType A comparator function type that can be passed to 
+   * SerialListener::listenFor or SerialListener::listenForOnce.
+   *
+   * \see SerialListener::listenFor, SerialListener::listenForOnce, 
+   * serial::ComparatorType
+   */
+  static ComparatorType
+  exactly (std::string exact_str);
+
+  // exact comparator function
+  static bool
+  _exactly (const std::string&, std::string);
+
+  /*!
+   * This returns a comparator that looks for a given prefix.
+   * 
+   * This can be used with listenFor or listenForOnce:
+   * 
+   * Example:
+   * <pre>
+   *   my_listener.listenFor(SerialListener::startsWith("V="), my_callback);
+   * <\pre>
+   * 
+   * \param prefix A std::string that is used as the prefix string to match 
+   * when comparing tokens for matching.
+   * 
+   * \return ComparatorType A comparator function type that can be passed to 
+   * SerialListener::listenFor or SerialListener::listenForOnce.
+   *
+   * \see SerialListener::listenFor, SerialListener::listenForOnce, 
+   * serial::ComparatorType
+   */
+  static ComparatorType
+  startsWith (std::string prefix) {
+    return boost::bind(&SerialListener::_startsWith, _1, prefix);
+  }
+
+  // exact comparator function
+  static bool
+  _startsWith (const std::string& token, std::string prefix) {
+    return token.substr(0,prefix.length()) == prefix;
+  }
+
+  /*!
+   * This returns a comparator that looks for a given postfix.
+   * 
+   * This can be used with listenFor or listenForOnce:
+   * 
+   * Example:
+   * <pre>
+   *   my_listener.listenFor(SerialListener::endsWith(";"), my_callback);
+   * <\pre>
+   * 
+   * \param postfix A std::string that is used as the postfix string to match 
+   * when comparing tokens for matching.
+   * 
+   * \return ComparatorType A comparator function type that can be passed to 
+   * SerialListener::listenFor or SerialListener::listenForOnce.
+   *
+   * \see SerialListener::listenFor, SerialListener::listenForOnce, 
+   * serial::ComparatorType
+   */
+  static ComparatorType
+  endsWith (std::string postfix) {
+    return boost::bind(&SerialListener::_endsWith, _1, postfix);
+  }
+
+  // endswith comparator function
+  static bool
+  _endsWith (const std::string& token, std::string postfix) {
+    return token.substr(token.length()-postfix.length()) == postfix;
+  }
+
+  /*!
+   * This returns a comparator that looks for a given substring in the token.
+   * 
+   * This can be used with listenFor or listenForOnce:
+   * 
+   * Example:
+   * <pre>
+   *   my_listener.listenFor(SerialListener::contains("some string"),
+   *                         my_callback);
+   * <\pre>
+   * 
+   * \param substr A std::string that is used as the search substring to match 
+   * when comparing tokens for matching.
+   * 
+   * \return ComparatorType A comparator function type that can be passed to 
+   * SerialListener::listenFor or SerialListener::listenForOnce.
+   *
+   * \see SerialListener::listenFor, SerialListener::listenForOnce, 
+   * serial::ComparatorType
+   */
+  static ComparatorType
+  contains (std::string substr) {
+    return boost::bind(&SerialListener::_contains, _1, substr);
+  }
+
+  // contains comparator function
+  static bool
+  _contains (const std::string& token, std::string substr) {
+    return token.find(substr) != std::string::npos;
+  }
+
 private:
   // Gets some data from the serial port
   void readSomeData (std::string&, size_t);
   // Takes newly tokenized data and processes them
   void addNewTokens(std::vector<std::string> &new_tokens,
-                    std::vector<uuid_type> new_uuids,
+                    std::vector<uuid_type> &new_uuids,
                     std::string &left_overs);
   // Runs the new tokens through the filters
-  void filter (std::vector<uuid_type> new_uuids);
+  void filterNewTokens (std::vector<uuid_type> new_uuids);
+  // Runs a list of tokens through one filter
+  std::vector<std::pair<uuid_type,uuid_type> >
+  filter(uuid_type filter_uuid, std::vector<uuid_type> &token_uuids);
   // Function that loops while listening is true
   void listen ();
   // Target of callback thread
@@ -489,8 +640,9 @@ private:
   void eraseTokens (std::vector<uuid_type>&);
   // Determines how much to read on each loop of listen
   size_t determineAmountToRead ();
-  // Used in the look for string once function
-  bool listenForOnceComparator (std::string line);
+  // Hanlder for listen for once
+  typedef boost::shared_ptr<boost::condition_variable> shared_cond_var_ptr_t;
+  void notifyListenForOnce (shared_cond_var_ptr_t cond_ptr);
 
   // Tokenizer
   TokenizerType tokenize;
@@ -512,34 +664,30 @@ private:
   boost::thread listen_thread;
   std::string data_buffer;
   boost::mutex token_mux;
-  std::map<const uuid_type,std::string> tokens;
-  std::map<const uuid_type,boost::posix_time::ptime> ttls;
+  std::map<const uuid_type, std::string> tokens;
+  std::map<const uuid_type, boost::posix_time::ptime> ttls;
 
   // Callback related variables
-  // uuid and true for default handler, false for normal callback
-  ConcurrentQueue<std::pair<uuid_type,bool> > callback_queue;
+  // filter uuid, token uuid
+  ConcurrentQueue<std::pair<uuid_type,uuid_type> > callback_queue;
   boost::thread callback_thread;
 
   // For generating random uuids
   boost::uuids::random_generator random_generator;
+  boost::uuids::nil_generator nil_generator;
 
   // Setting for ttl on messages
   boost::posix_time::time_duration ttl;
 
   // map<uuid, filter type (blocking/non-blocking)>
-  std::map<const uuid_type,filter_type::FilterType> filters;
+  std::vector<uuid_type> filters;
   // map<uuid, comparator>
-  std::map<const uuid_type,ComparatorType> comparators;
+  std::map<const uuid_type, ComparatorType> comparators;
   // map<uuid, callback>
-  std::map<const uuid_type,DataCallback> callbacks;
-  // map<uuid, conditional_variables>
-  std::map<const uuid_type,boost::condition_variable*> condition_vars;
+  std::map<const uuid_type, DataCallback> callbacks;
   // Mutex for locking use of filters
   boost::mutex filter_mux;
   boost::mutex callback_mux;
-
-  // Used as temporary storage for listenForStringOnce
-  std::string current_listen_for_one_target;
 };
 
 }
diff --git a/src/serial.cc b/src/serial.cc
index ae37ff1..c304ee3 100644
--- a/src/serial.cc
+++ b/src/serial.cc
@@ -170,7 +170,8 @@ void Serial::open() {
     
     // Try to open the serial port
     try {
-        this->serial_port.reset(new boost::asio::serial_port(this->io_service, this->port));
+        this->serial_port.reset(
+              new boost::asio::serial_port(this->io_service, this->port));
         
         this->serial_port->set_option(this->baudrate);
         this->serial_port->set_option(this->flowcontrol);
@@ -199,7 +200,8 @@ void Serial::close() {
     }
 }
 
-static const boost::posix_time::time_duration timeout_zero_comparison(boost::posix_time::milliseconds(0));
+static const boost::posix_time::time_duration 
+timeout_zero_comparison(boost::posix_time::milliseconds(0));
 
 int Serial::read(char* buffer, int size) {
     this->reading = true;
diff --git a/src/serial_listener.cc b/src/serial_listener.cc
index c3becd0..77993f0 100644
--- a/src/serial_listener.cc
+++ b/src/serial_listener.cc
@@ -48,34 +48,37 @@ SerialListener::~SerialListener() {
 void
 SerialListener::callback() {
   try {
-    std::pair<uuid_type,bool> pair;
+    // <filter uuid, token uuid>
+    std::pair<uuid_type,uuid_type> pair;
     DataCallback _callback;
     while (this->listening) {
       if (this->callback_queue.timed_wait_and_pop(pair, 10)) {
         if (this->listening) {
-          std::cout << "After pop (" << pair.second << "): ";
-          std::cout << this->tokens[pair.first] << std::endl;
           try {
             // If default handler
-            if (pair.second) {
+            if (pair.first.is_nil()) {
               if (this->default_handler)
-                this->default_handler(this->tokens[pair.first]);
+                this->default_handler(this->tokens[pair.second]);
             // Else use provided callback
             } else {
+              bool go = false;
               // Grab the callback as to not hold the mutex while executing
               {
                 boost::mutex::scoped_lock l(callback_mux);
-                _callback = this->callbacks[pair.first];
+                // Make sure the filter hasn't been removed
+                if ((go = (this->callbacks.count(pair.first) > 0)))
+                  _callback = this->callbacks[pair.first];
               }
               // Execute callback
-              _callback(this->tokens[pair.first]);
+              if (go)
+                _callback(this->tokens[pair.second]);
             }
           } catch (std::exception &e) {
             this->handle_exc(e);
           }// try callback
         } // if listening
         // Erase the used and executed callback
-        this->eraseToken(pair.first);
+        this->eraseToken(pair.second);
       } // if popped
     } // while (this->listening)
   } catch (std::exception &e) {
@@ -90,14 +93,14 @@ SerialListener::setTimeToLive(size_t ms) {
 }
 
 void
-SerialListener::startListening(Serial * serial_port) {
+SerialListener::startListening(Serial &serial_port) {
   if (this->listening) {
     throw(SerialListenerException("Already listening."));
     return;
   }
   this->listening = true;
   
-  this->serial_port = serial_port;
+  this->serial_port = &serial_port;
   if (!this->serial_port->isOpen()) {
     throw(SerialListenerException("Serial port not open."));
     return;
@@ -131,11 +134,11 @@ SerialListener::stopListening() {
 void
 SerialListener::stopListeningForAll() {
   boost::mutex::scoped_lock l(filter_mux);
-  boost::mutex::scoped_lock l2(callback_mux);
   filters.clear();
   comparators.clear();
-  condition_vars.clear();
+  boost::mutex::scoped_lock l2(callback_mux);
   callbacks.clear();
+  callback_queue.clear();
 }
 
 size_t
@@ -143,7 +146,7 @@ SerialListener::determineAmountToRead() {
   // TODO: Make a more intelligent method based on the length of the things 
   //  filters are looking for.  i.e.: if the filter is looking for 'V=XX\r' 
   //  make the read amount at least 5.
-  return 5;
+  return 1024;
 }
 
 void
@@ -161,14 +164,11 @@ SerialListener::readSomeData(std::string &temp, size_t this_many) {
 
 void
 SerialListener::addNewTokens(std::vector<std::string> &new_tokens,
-                             std::vector<uuid_type> new_uuids,
+                             std::vector<uuid_type> &new_uuids,
                              std::string &left_overs)
 {
-  std::cout << "Inside SerialListener::addNewTokens:" << std::endl;
-  // Iterate through new tokens and add times to them
   std::vector<std::string>::iterator it_new;
   for (it_new=new_tokens.begin(); it_new != new_tokens.end(); it_new++) {
-    std::cout << "  Token (" << (*it_new).length() << "): " << (*it_new) << std::endl;
     // The last token needs to be put back in the buffer always:
     //  (in the case that the delimeter is \r)...
     //  In the case that the string ends with \r the last element will be 
@@ -180,6 +180,9 @@ SerialListener::addNewTokens(std::vector<std::string> &new_tokens,
       left_overs = (*it_new);
       continue;
     }
+    // If the token is empty ignore it
+    if ((*it_new).length() == 0)
+      continue;
     // Create a new uuid
     uuid_type uuid = random_generator();
     // Put the new uuid in the list of new uuids
@@ -219,59 +222,40 @@ SerialListener::eraseTokens(std::vector<uuid_type> &uuids) {
 void
 SerialListener::filterNewTokens(std::vector<uuid_type> new_uuids) {
   // Iterate through the filters, checking each against new tokens
+  std::vector<std::pair<uuid_type,uuid_type> > tbd;
   boost::mutex::scoped_lock l(filter_mux);
-  std::map<const uuid_type,filter_type::FilterType>::iterator it;
+  std::vector<uuid_type>::iterator it;
   for (it=filters.begin(); it!=filters.end(); it++) {
-    this->filter((*it).first, new_uuids);
+    std::vector<std::pair<uuid_type,uuid_type> > temp =
+      this->filter((*it), new_uuids);
+    if (temp.size() > 0)
+      tbd.insert(tbd.end(), temp.begin(), temp.end());
   } // for (it=filters.begin(); it!=filters.end(); it++)
-  // Get the filter lock
-  boost::mutex::scoped_lock l(filter_mux);
-  std::vector<uuid_type> to_be_erased;
-  // Iterate through the tokens checking for a match
-  std::vector<uuid_type>::iterator it_uuids;
-  for (it_uuids=new_uuids.begin(); it_uuids!=new_uuids.end(); it_uuids++) {
-    bool matched = false;
-    uuid_type uuid = (*it_uuids);
-    // If the line is empty, continue
-    if (tokens[uuid].length() == 0) {
-      continue;
-    }
-    // Iterate through each filter
-    std::map<const uuid_type,filter_type::FilterType>::iterator it;
-    for (it=filters.begin(); it!=filters.end(); it++) {
-      // If comparator matches line
-      if (comparators[(*it).first](tokens[uuid])) {
-        // If non-blocking run the callback
-        if ((*it).second == filter_type::nonblocking) {
-          callback_queue.push(std::pair<uuid_type,bool>(uuid,false));
-        // If blocking then notify the waiting call to continue
-        } else if ((*it).second == filter_type::blocking) {
-          condition_vars[(*it).first]->notify_all();
-          to_be_erased.push_back(uuid);
-        }
-        matched = true;
-        break; // It matched, continue to next line
-      }
-    } // for(it=filters.begin(); it!=filters.end(); it++)
-  } // for(it_lines=lines.begin(); it_lines!=lines.end(); it_lines++)
-  // Remove any lines that need to be erased
-  //  (this must be done outside the iterator to prevent problems incrementing
-  //   the iterator)
-  this->eraseTokens(to_be_erased);
+  // Dispatch
+  std::vector<std::pair<uuid_type,uuid_type> >::iterator it_tbd;
+  for (it_tbd = tbd.begin(); it_tbd != tbd.end(); it_tbd++) {
+    callback_queue.push((*it_tbd));
+  }
 }
 
-void
-filter(uuid_type filter_uuid, std::vector<uuid_type> token_uuids) {
+// <filter,token>
+std::vector<std::pair<uuid_type,uuid_type> >
+SerialListener::filter(uuid_type filter_uuid,
+                       std::vector<uuid_type> &token_uuids)
+{
   std::vector<uuid_type> to_be_erased;
+  std::vector<std::pair<uuid_type,uuid_type> > to_be_dispatched;
   // Iterate through the token uuids and run each against the filter
-  std::vector<uuid_type>::iterator it_uuids;
-  for (it_uuids=new_uuids.begin(); it_uuids!=new_uuids.end(); it_uuids++) {
-    
+  std::vector<uuid_type>::iterator it;
+  for (it=token_uuids.begin(); it!=token_uuids.end(); it++) {
+    bool matched = false;
+    uuid_type token_uuid = (*it);
+    if (this->comparators[filter_uuid](this->tokens[token_uuid])) {
+      matched = true;
+      to_be_dispatched.push_back(std::make_pair(filter_uuid,token_uuid));
+    }
   }
-  // Remove any lines that need to be erased
-  //  (this must be done outside the iterator to prevent problems incrementing
-  //   the iterator)
-  this->eraseTokens(to_be_erased);
+  return to_be_dispatched;
 }
 
 void
@@ -288,12 +272,10 @@ SerialListener::pruneTokens() {
       // If the current time - the creation time is greater than the ttl, 
       //  then prune it
       if (ptime(microsec_clock::local_time())-this->ttls[uuid] > this->ttl) {
-        std::cout << "Pruning (" << this->tokens[uuid].length();
-        std::cout << "): " << this->tokens[uuid] << std::endl;
         // If there is a default handler pass it on
         if (this->default_handler) {
           boost::mutex::scoped_lock l(callback_mux);
-          callback_queue.push(std::pair<uuid_type,bool>(uuid,true));
+          callback_queue.push(std::make_pair(nil_generator(),uuid));
         } else {
           // Otherwise delete it
           to_be_erased.push_back(uuid);
@@ -339,49 +321,80 @@ SerialListener::listen() {
 }
 
 bool
-SerialListener::listenForOnceComparator(std::string token) {
-  if (token == current_listen_for_one_target)
-    return true;
-  return false;
+SerialListener::listenForStringOnce(std::string token, size_t milliseconds) {
+  return this->listenForOnce(exactly(token), milliseconds);
+}
+
+void
+SerialListener::notifyListenForOnce(shared_cond_var_ptr_t cond_ptr) {
+  cond_ptr->notify_all();
 }
 
 bool
-SerialListener::listenForStringOnce(std::string token, size_t milliseconds) {
-  boost::condition_variable cond;
+SerialListener::listenForOnce(ComparatorType comparator, size_t ms)
+{
+  shared_cond_var_ptr_t cond_ptr(new boost::condition_variable());
   boost::mutex mut;
-  current_listen_for_one_target = token;
 
   // Create blocking filter
-  uuid_type uuid = random_generator();
-  std::pair<const uuid_type,filter_type::FilterType>
-   filter_pair(uuid, filter_type::blocking);
-  std::pair<const uuid_type,ComparatorType>
-   comparator_pair(uuid,
-    boost::bind(&SerialListener::listenForOnceComparator, this, _1));
-  std::pair<const uuid_type,boost::condition_variable*>
-   condition_pair(uuid, &cond);
+  const uuid_type uuid = random_generator();
   {
     boost::mutex::scoped_lock l(filter_mux);
-    filters.insert(filter_pair);
-    comparators.insert(comparator_pair);
-    condition_vars.insert(condition_pair);
+    filters.push_back(uuid);
+    comparators.insert(std::make_pair(uuid,comparator));
+  }
+  {
+    boost::mutex::scoped_lock l(callback_mux);
+    callbacks.insert(std::make_pair(uuid,
+      boost::bind(&SerialListener::notifyListenForOnce, this, cond_ptr)));
   }
 
-  this->processNewFilter(uuid);
+  // Run this filter through all tokens onces
+  std::vector<uuid_type> token_uuids;
+  std::map<const uuid_type,std::string>::iterator it;
+  for (it = tokens.begin(); it != tokens.end(); it++)
+    token_uuids.push_back((*it).first);
+  std::vector<std::pair<uuid_type,uuid_type> > pairs =
+    this->filter(uuid, token_uuids);
+
+  // If there is at least one
+  if (pairs.size() > 0) {
+    // If there is more than one find the oldest
+    size_t index = 0;
+    if (pairs.size() > 1) {
+      using namespace boost::posix_time;
+      ptime oldest_time = ttls[pairs[index].second];
+      size_t i = 0;
+      std::vector<std::pair<uuid_type,uuid_type> >::iterator it;
+      for (it = pairs.begin(); it != pairs.end(); it++) {
+        if (ttls[(*it).second] < oldest_time) {
+          oldest_time = ttls[(*it).second];
+          index = i;
+        }
+        i++;
+      }
+    }
+    // Either way put the final index into the callback queue
+    callback_queue.push(pairs[index]);
+  }
 
   bool result = false;
 
   // Wait
   boost::unique_lock<boost::mutex> lock(mut);
-  if (cond.timed_wait(lock, boost::posix_time::milliseconds(milliseconds)))
+  using namespace boost::posix_time;
+  if (cond_ptr->timed_wait(lock, milliseconds(ms)))
     result = true;
 
   // Destroy the filter
   {
     boost::mutex::scoped_lock l(filter_mux);
-    filters.erase(uuid);
+    filters.erase(std::find(filters.begin(),filters.end(),uuid));
     comparators.erase(uuid);
-    condition_vars.erase(uuid);
+  }
+  {
+    boost::mutex::scoped_lock l(callback_mux);
+    callbacks.erase(uuid);
   }
 
   return result;
@@ -392,8 +405,6 @@ SerialListener::listenFor(ComparatorType comparator, DataCallback callback)
 {
   // Create Filter
   uuid_type uuid = random_generator();
-  std::pair<const uuid_type,filter_type::FilterType>
-   filter_pair(uuid, filter_type::nonblocking);
   std::pair<const uuid_type,ComparatorType>
    comparator_pair(uuid, comparator);
   std::pair<const uuid_type,DataCallback>
@@ -401,7 +412,7 @@ SerialListener::listenFor(ComparatorType comparator, DataCallback callback)
 
   {
     boost::mutex::scoped_lock l(filter_mux);
-    filters.insert(filter_pair);
+    filters.push_back(uuid);
     comparators.insert(comparator_pair);
   }
   {
@@ -409,6 +420,20 @@ SerialListener::listenFor(ComparatorType comparator, DataCallback callback)
     callbacks.insert(callback_pair);
   }
 
+  // Run this filter through all tokens onces
+  std::vector<uuid_type> token_uuids;
+  std::map<const uuid_type,std::string>::iterator it;
+  for (it = tokens.begin(); it != tokens.end(); it++)
+    token_uuids.push_back((*it).first);
+  std::vector<std::pair<uuid_type,uuid_type> > pairs =
+    this->filter(uuid, token_uuids);
+
+  // Dispatch
+  std::vector<std::pair<uuid_type,uuid_type> >::iterator it_cb;
+  for (it_cb = pairs.begin(); it_cb != pairs.end(); it_cb++) {
+    callback_queue.push((*it_cb));
+  }
+
   return uuid;
 }
 
@@ -416,8 +441,9 @@ void
 SerialListener::stopListeningFor(uuid_type filter_uuid) {
   // Delete filter
   boost::mutex::scoped_lock l(filter_mux);
-  filters.erase(filter_uuid);
+  filters.erase(std::find(filters.begin(),filters.end(),filter_uuid));
   comparators.erase(filter_uuid);
+  boost::mutex::scoped_lock l2(callback_mux);
   callbacks.erase(filter_uuid);
 }
 
@@ -435,3 +461,13 @@ SerialListener::_delimeter_tokenizer (std::string &data,
   boost::split(tokens, data, boost::is_any_of(delimeter));
 }
 
+ComparatorType
+SerialListener::exactly(std::string exact_str) {
+  return boost::bind(&SerialListener::_exactly, _1, exact_str);
+}
+
+bool
+SerialListener::_exactly(const std::string &token, std::string exact_str) {
+  return token == exact_str;
+}
+
diff --git a/tests/serial_listener_tests.cc b/tests/serial_listener_tests.cc
index f589b04..81e4aef 100644
--- a/tests/serial_listener_tests.cc
+++ b/tests/serial_listener_tests.cc
@@ -13,7 +13,7 @@ static size_t global_count, global_listen_count;
 
 void default_handler(std::string line) {
   global_count++;
-  std::cout << "default_handler got: " << line << std::endl;
+  // std::cout << "default_handler got: " << line << std::endl;
 }
 
 namespace {
@@ -45,7 +45,7 @@ protected:
   }
 
   void execute_listenForStringOnce() {
-    listener.listenForStringOnce("?$1E", 1000);
+    listener.listenForStringOnce("?$1E", 50);
   }
 
   void simulate_loop(std::string input_str) {
@@ -80,11 +80,11 @@ TEST_F(SerialListenerTests, listenForOnceWorks) {
   boost::thread t(
     boost::bind(&SerialListenerTests::execute_listenForStringOnce, this));
 
-  boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+  boost::this_thread::sleep(boost::posix_time::milliseconds(5));
 
   simulate_loop("\r+\r?$1E\r$1E=Robo");
 
-  ASSERT_TRUE(t.timed_join(boost::posix_time::milliseconds(1500)));
+  ASSERT_TRUE(t.timed_join(boost::posix_time::milliseconds(60)));
 
   // Make sure the filters are getting deleted
   ASSERT_EQ(listener.filters.size(), 0);
@@ -92,7 +92,7 @@ TEST_F(SerialListenerTests, listenForOnceWorks) {
   // give some time for the callback thread to finish
   stopCallbackThread();
 
-  ASSERT_EQ(global_count, 2);
+  ASSERT_EQ(global_count, 1);
 }
 
 // lookForOnce should not find it, but timeout after 1000ms, so it should 
@@ -103,11 +103,11 @@ TEST_F(SerialListenerTests, listenForOnceTimesout) {
   boost::thread t(
     boost::bind(&SerialListenerTests::execute_listenForStringOnce, this));
 
-  boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+  boost::this_thread::sleep(boost::posix_time::milliseconds(55));
 
   simulate_loop("\r+\r?$1ENOTRIGHT\r$1E=Robo");
 
-  ASSERT_TRUE(t.timed_join(boost::posix_time::milliseconds(1500)));
+  ASSERT_TRUE(t.timed_join(boost::posix_time::milliseconds(60)));
 
   // give some time for the callback thread to finish
   stopCallbackThread();
@@ -116,6 +116,7 @@ TEST_F(SerialListenerTests, listenForOnceTimesout) {
 }
 
 bool listenForComparator(std::string line) {
+  // std::cout << "In listenForComparator(" << line << ")" << std::endl;
   if (line.substr(0,2) == "V=") {
     return true;
   }
@@ -123,6 +124,7 @@ bool listenForComparator(std::string line) {
 }
 
 void listenForCallback(std::string line) {
+  // std::cout << "In listenForCallback(" << line << ")" << std::endl;
   global_listen_count++;
 }