diff --git a/tools/cooja/java/se/sics/cooja/EventQueue.java b/tools/cooja/java/se/sics/cooja/EventQueue.java
index 84d496c33..bc58bf606 100644
--- a/tools/cooja/java/se/sics/cooja/EventQueue.java
+++ b/tools/cooja/java/se/sics/cooja/EventQueue.java
@@ -26,7 +26,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: EventQueue.java,v 1.3 2008/12/08 13:07:32 fros4943 Exp $
+ * $Id: EventQueue.java,v 1.4 2009/01/08 15:42:25 fros4943 Exp $
  */
 
 package se.sics.cooja;
@@ -39,42 +39,61 @@ import java.util.ArrayList;
 public class EventQueue {
 
   private TimeEvent first;
-  private long nextTime;
   private int eventCount = 0;
 
-  private ArrayList<TimeEvent> pendingEvents = new ArrayList<TimeEvent>();
-  private boolean hasPendingEvents = false;
+  /* For scheduling events from outside simulation thread effectively */
+  private boolean hasUnsortedEvents = false;
+  private ArrayList<TimeEvent> unsortedEvents = new ArrayList<TimeEvent>();
 
   public EventQueue() {
   }
 
-  public synchronized void addPendingEvent(TimeEvent event, long time) {
-    event.time = time;
-    pendingEvents.add(event);
-    hasPendingEvents  = true;
-  }
+  private synchronized void sortEvents() {
+    hasUnsortedEvents = false;
 
-  private synchronized void handlePendingEvents() {
-    if (!hasPendingEvents) {
-      return;
-    }
-
-    for (TimeEvent e: pendingEvents) {
+    for (TimeEvent e: unsortedEvents) {
       addEvent(e);
     }
-    pendingEvents.clear();
-    hasPendingEvents = false;
+    unsortedEvents.clear();
   }
 
+  /**
+   * May be called from outside simulation thread.
+   *
+   * @param event Event
+   * @param time Time
+   */
+  public synchronized void addEventUnsorted(TimeEvent event, long time) {
+    /* Make sure this event is not executed before being resorted (readded) */
+    if (event.queue != null) {
+      event.remove();
+    }
+    event.time = time;
+    unsortedEvents.add(event);
+    hasUnsortedEvents = true;
+  }
+
+  /**
+   * Should only be called from simulation thread!
+   *
+   * @param event Event
+   * @param time Time
+   */
   public void addEvent(TimeEvent event, long time) {
     event.time = time;
     addEvent(event);
   }
 
+  /**
+   * Should only be called from simulation thread!
+   *
+   * @param event Event
+   */
   public void addEvent(TimeEvent event) {
-    if (event.scheduledIn != null) {
-      event.remove();
+    if (event.queue != null) {
+      removeFromQueue(event);
     }
+
     if (first == null) {
       first = event;
     } else {
@@ -95,21 +114,21 @@ public class EventQueue {
         lastPos.nextEvent = event;
       }
     }
-    if (first != null) {
-      nextTime = first.time;
-    } else {
-      nextTime = 0;
-    }
-    event.scheduledIn = this;
+    event.removed = false;
+    event.queue = this;
     eventCount++;
   }
 
-  // Not yet impl.
-  public boolean removeEvent(TimeEvent event) {
+  /**
+   * Should only be called from simulation thread!
+   *
+   * @param event Event
+   * @return True if event was removed
+   */
+  private boolean removeFromQueue(TimeEvent event) {
     TimeEvent pos = first;
     TimeEvent lastPos = first;
-//  System.out.println("Removing: " + event.getShort() + "  Before remove: ");
-//  print();
+
     while (pos != null && pos != event) {
       lastPos = pos;
       pos = pos.nextEvent;
@@ -128,21 +147,26 @@ public class EventQueue {
     // unlink
     pos.nextEvent = null;
 
-    if (first != null) {
-      nextTime = first.time;
-    } else {
-      nextTime = 0;
-    }
-//  System.out.println("Removed =>");
-//  print();
-    event.scheduledIn = null;
+    event.queue = null;
     eventCount--;
     return true;
   }
 
+  public void removeAll() {
+    TimeEvent event = popFirst();
+    while (event != null) {
+      event = popFirst();
+    }
+  }
+
+  /**
+   * Should only be called from simulation thread!
+   *
+   * @return Event
+   */
   public TimeEvent popFirst() {
-    if (hasPendingEvents) {
-      handlePendingEvents();
+    if (hasUnsortedEvents) {
+      sortEvents();
     }
 
     TimeEvent tmp = first;
@@ -154,31 +178,18 @@ public class EventQueue {
     // Unlink.
     tmp.nextEvent = null;
 
-    if (first != null) {
-      nextTime = first.time;
-    } else {
-      nextTime = 0;
-    }
     // No longer scheduled!
-    tmp.scheduledIn = null;
+    tmp.queue = null;
     eventCount--;
+
+    if (tmp.removed) {
+      /* pop and return another event instead */
+      return popFirst();
+    }
     return tmp;
   }
 
-  public void removeAll() {
-    TimeEvent t = first;
-    while(t != null) {
-      TimeEvent clr = t;
-      t = t.nextEvent;
-      clr.nextEvent = null;
-      clr.time = 0;
-      clr.scheduledIn = null;
-    }
-    first = null;
-    eventCount = 0;
-  }
-
   public String toString() {
-    return "EventQueue (" + eventCount + " events)";
+    return "EventQueue with " + (eventCount+unsortedEvents.size()) + " events";
   }
-} // LLEventQueue
+}
diff --git a/tools/cooja/java/se/sics/cooja/Simulation.java b/tools/cooja/java/se/sics/cooja/Simulation.java
index 5fe00efc0..269b244a7 100644
--- a/tools/cooja/java/se/sics/cooja/Simulation.java
+++ b/tools/cooja/java/se/sics/cooja/Simulation.java
@@ -24,7 +24,7 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * $Id: Simulation.java,v 1.36 2008/12/08 13:07:06 fros4943 Exp $
+ * $Id: Simulation.java,v 1.37 2009/01/08 15:42:38 fros4943 Exp $
  */
 
 package se.sics.cooja;
@@ -126,7 +126,7 @@ public class Simulation extends Observable implements Runnable {
     if (Thread.currentThread() == simulationThread) {
       eventQueue.addEvent(e, time);
     } else {
-      eventQueue.addPendingEvent(e, time);
+      eventQueue.addEventUnsorted(e, time);
     }
   }
 
diff --git a/tools/cooja/java/se/sics/cooja/TimeEvent.java b/tools/cooja/java/se/sics/cooja/TimeEvent.java
index 662353c5e..2098704dd 100644
--- a/tools/cooja/java/se/sics/cooja/TimeEvent.java
+++ b/tools/cooja/java/se/sics/cooja/TimeEvent.java
@@ -26,7 +26,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: TimeEvent.java,v 1.2 2008/12/04 14:03:42 joxe Exp $
+ * $Id: TimeEvent.java,v 1.3 2009/01/08 15:42:25 fros4943 Exp $
  */
 
 package se.sics.cooja;
@@ -35,18 +35,18 @@ package se.sics.cooja;
  * @author Joakim Eriksson (ported to COOJA by Fredrik Österlind)
  */
 public abstract class TimeEvent {
-  // For linking events...
   TimeEvent nextEvent;
   TimeEvent prevEvent;
 
-  // Keeps track of where this is scheduled
-  EventQueue scheduledIn = null;
+  EventQueue queue = null;
   String name;
 
   protected long time;
 
+  boolean removed = false;
+
   public TimeEvent(long time) {
-    this.time = time;
+    this(time, null);
   }
 
   public TimeEvent(long time, String name) {
@@ -59,13 +59,11 @@ public abstract class TimeEvent {
   }
 
   public boolean isScheduled() {
-    return scheduledIn != null;
+    return queue != null && !removed;
   }
 
   public boolean remove() {
-    if (scheduledIn != null) {
-      return scheduledIn.removeEvent(this);
-    }
+    removed = true;
     return false;
   }
 
@@ -75,4 +73,4 @@ public abstract class TimeEvent {
     return "" + time + (name != null ? ": " + name : "");
   }
 
-} // TimeEvent
+}