diff --git a/model/pit/ndn-pit-entry.h b/model/pit/ndn-pit-entry.h
index d95d338..1dc4264 100644
--- a/model/pit/ndn-pit-entry.h
+++ b/model/pit/ndn-pit-entry.h
@@ -313,13 +313,13 @@
        item ++)
     {
       boost::shared_ptr< T > retPtr = boost::dynamic_pointer_cast<T> (*item);
-      if (retPtr != 0)
+      if (retPtr != boost::shared_ptr< T > ())
         {
           return retPtr;
         }
     }
 
-  return 0;
+  return boost::shared_ptr< T > ();
 }
 
 // /**
@@ -352,7 +352,7 @@
        item ++)
     {
       boost::shared_ptr< T > retPtr = boost::dynamic_pointer_cast< T > (*item);
-      if (retPtr != 0)
+      if (retPtr != boost::shared_ptr< T > ())
         {
           m_fwTags.erase (item);
           return;
diff --git a/test/generic-tests.cc b/test/generic-tests.cc
index 2e751f7..652a7f8 100644
--- a/test/generic-tests.cc
+++ b/test/generic-tests.cc
@@ -28,8 +28,11 @@
 #include "../apps/ndn-producer.h"
 
 #include <boost/lexical_cast.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
 
 using namespace std;
+using namespace boost;
 
 NS_LOG_COMPONENT_DEFINE ("ndn.test.Generic");
 
@@ -83,6 +86,20 @@
     {
       cout << "* " << entry->GetPrefix () << endl;
     }
+
+
+  cerr << "===========\n\n";
+
+  struct Bla {
+  };
+
+  shared_ptr<Bla> p1 = make_shared <Bla> ();
+  shared_ptr<Bla> p2 = make_shared <Bla> ();
+
+  if (p1 < p2)
+    {
+      cerr << "They are equal\n";
+    }
 }
 
 } // namespace ndn
diff --git a/utils/ndn-pit-queue.cc b/utils/ndn-pit-queue.cc
index 4d6c064..29551a8 100644
--- a/utils/ndn-pit-queue.cc
+++ b/utils/ndn-pit-queue.cc
@@ -26,6 +26,7 @@
 #include "ns3/assert.h"
 
 using namespace std;
+using namespace boost;
 
 namespace ns3 {
 namespace ndn {
@@ -65,10 +66,19 @@
   if (queue->second->size () >= m_maxQueueSize)
       return false;
 
-  queue->second->push_back (pitEntry);
+  Queue::iterator itemIterator = queue->second->insert (queue->second->end (), pitEntry);
+  
+  shared_ptr<fw::PitQueueTag> tag = pitEntry->GetFwTag<fw::PitQueueTag> ();
+  if (tag == shared_ptr<fw::PitQueueTag> ())
+    {
+      tag = make_shared<fw::PitQueueTag> ();
+      pitEntry->AddFwTag (tag);
+    }
+  tag->InsertQueue (queue->second, itemIterator);
   return true;
 }
 
+
 Ptr<pit::Entry>
 PitQueue::Pop ()
 {
@@ -93,8 +103,17 @@
   NS_ASSERT_MSG (queue->second->size () != 0, "Logic error");
 
   Ptr<pit::Entry> entry = *queue->second->begin ();
-  queue->second->pop_front ();
+  shared_ptr<fw::PitQueueTag> tag = entry->GetFwTag<fw::PitQueueTag> ();
+  NS_ASSERT (tag != shared_ptr<fw::PitQueueTag> ());
 
+#ifdef NS3_LOG_ENABLE
+  size_t queueSize = queue->second->size ();
+#endif
+  tag->RemoveFromQueue (queue->second);
+#ifdef NS3_LOG_ENABLE
+  NS_ASSERT_MSG (queue->second->size () == queueSize-1, "Queue size should be reduced by one");
+#endif
+    
   m_lastQueue = queue;
   return entry;
 }
@@ -107,14 +126,63 @@
       m_lastQueue++;
     }
 
-  m_queues.erase (face);
+  PerInFaceQueue::iterator queue = m_queues.find (face);
+  if (queue == m_queues.end ())
+    return;
+
+  for (Queue::iterator pitEntry = queue->second->begin ();
+       pitEntry != queue->second->end ();
+       pitEntry ++)
+    {
+      shared_ptr<fw::PitQueueTag> tag = (*pitEntry)->GetFwTag<fw::PitQueueTag> ();
+      NS_ASSERT (tag != shared_ptr<fw::PitQueueTag> ());
+
+      tag->RemoveFromQueue (queue->second);
+    }
+
+  NS_ASSERT_MSG (queue->second->size () == 0, "Queue size should be 0 by now");
+  m_queues.erase (queue);
 }
 
 void
 PitQueue::Remove (Ptr<pit::Entry> entry)
 {
+  shared_ptr<fw::PitQueueTag> tag = entry->GetFwTag<fw::PitQueueTag> ();
+  if (tag == shared_ptr<fw::PitQueueTag> ())
+    return;
+
+  tag->RemoveFromAllQueues ();
 }
 
+void
+fw::PitQueueTag::InsertQueue (boost::shared_ptr<PitQueue::Queue> queue, PitQueue::Queue::iterator iterator)
+{
+  pair<MapOfItems::iterator, bool> item = m_items.insert (make_pair (queue, iterator));
+  NS_ASSERT (item.second == true);
+}
+
+void
+fw::PitQueueTag::RemoveFromAllQueues ()
+{
+  for (MapOfItems::iterator item = m_items.begin ();
+       item != m_items.end ();
+       item ++)
+    {
+      item->first->erase (item->second);
+    }
+  m_items.clear ();
+}
+
+void
+fw::PitQueueTag::RemoveFromQueue (boost::shared_ptr<PitQueue::Queue> queue)
+{
+  MapOfItems::iterator item = m_items.find (queue);
+  if (item == m_items.end ())
+    return;
+
+  item->first->erase (item->second);
+  m_items.erase (item);
+}
 
 } // namespace ndn
 } // namespace ns3
diff --git a/utils/ndn-pit-queue.h b/utils/ndn-pit-queue.h
index 34987db..ff4f0de 100644
--- a/utils/ndn-pit-queue.h
+++ b/utils/ndn-pit-queue.h
@@ -67,7 +67,6 @@
   typedef std::map< Ptr<Face>, boost::shared_ptr<Queue> > PerInFaceQueue;
 
 private:
-  
   uint32_t m_maxQueueSize;
   PerInFaceQueue::iterator m_lastQueue; // last queue from which interest was taken
   PerInFaceQueue m_queues;
@@ -79,10 +78,24 @@
     public Tag
 {
 public:
-  virtual
-  ~PitQueueTag () { };
+  // map based on addresses, should be good enough
+  typedef std::map< boost::shared_ptr<PitQueue::Queue>, PitQueue::Queue::iterator > MapOfItems;
 
-  typedef boost::tuple< boost::shared_ptr<PitQueue::Queue>, PitQueue::Queue::iterator > Item;
+public:
+  virtual
+  ~PitQueueTag () { };  
+
+  void
+  InsertQueue (boost::shared_ptr<PitQueue::Queue> item, PitQueue::Queue::iterator iterator);
+  
+  void
+  RemoveFromAllQueues ();
+
+  void
+  RemoveFromQueue (boost::shared_ptr<PitQueue::Queue> queue);
+  
+private:
+  MapOfItems m_items;
 };
 
 } // namespace fw