diff --git a/src/thread/thread.c b/src/thread/thread.c
index 0389754..1acfa4c 100644
--- a/src/thread/thread.c
+++ b/src/thread/thread.c
@@ -24,7 +24,7 @@
 #define STACK_SIZE 4096
 #endif
 
-// Variables used to clean up everything at the end of the processus
+// Variables used to clean up everything at the end of the processes
 static char stack_for_freeing[STACK_SIZE] = {0};
 static int stack_valgrind_id = 0;
 static ucontext_t context_for_freeing;
@@ -43,14 +43,13 @@ struct context_entry {
 static TAILQ_HEAD(context_head, context_entry) head = TAILQ_HEAD_INITIALIZER(head);
 // Current running thread
 static struct context_entry* running = NULL;
-// Thread counter
-static unsigned long long counter = 0;
 
 int thread_yield(void)
 {
     TRACE("thread_yield");
-    if (counter <= 1)
+    if (TAILQ_EMPTY(&head)) {
         return 0;
+    }
 
     /* Current strategy :
      * if we have checked the number of threads then keep the running one
@@ -59,25 +58,14 @@ int thread_yield(void)
      * check if the thread is not finished and is not waiting for a non finished thread
      * check if the thread is not the running one.
      */
-    struct context_entry* first = NULL;
-    int count = 0;
-    do {
-        if (count++ == counter) {
-            return 0;
-        }
-
-        first = TAILQ_FIRST(&head);
-        if (!first) {
-            return -1;
-        }
-
-        TAILQ_REMOVE(&head, first, link);
-        TAILQ_INSERT_TAIL(&head, first, link);
-    } while (
-        IS_FINISHED(first)
-        || IS_WAITING(first) && !IS_WAITING_THREAD_FINISHED(first)
-        || (first->id == running->id));
-
+    struct context_entry* first = TAILQ_FIRST(&head);
+    if (!first) {
+        return -1;
+    }
+    TAILQ_REMOVE(&head, first, link);
+    if (!IS_FINISHED(running) && !(IS_WAITING(running) && !IS_WAITING_THREAD_FINISHED(running))) {
+        TAILQ_INSERT_TAIL(&head, running, link);
+    }
     TRACE("PICKING %p (previous was %p)", first->id, running->id);
     // Switch to the new thread.
     struct context_entry* old_runner = running;
@@ -134,7 +122,6 @@ int thread_create(thread_t* newthread, void* (*func)(void*), void* funcarg)
     *newthread = new_entry->id;
 
     makecontext(&new_entry->context, (void (*)(void))thread_function_wrapper, 2, func, funcarg);
-    counter++;
 
     TAILQ_INSERT_TAIL(&head, new_entry, link);
     return 0;
@@ -154,34 +141,34 @@ int thread_join(thread_t thread, void** retval)
         return -1;
     }
 
-    // Use status to be in waiting state
-    running->status |= WAITING;
-    // Use retvalue to share which thread we are currently waiting for
-    running->retvalue = entry;
-    // Mark the waited thread as waited to not be waited by any other thread.
-    entry->status |= WAITED;
+    if (!IS_FINISHED(entry)) {
+        // Use status to be in waiting state
+        running->status |= WAITING;
+        // Use retvalue to share which thread we are currently waiting for
+        running->retvalue = entry;
+        // Mark the waited thread as waited to not be waited by any other thread.
+        entry->status |= WAITED;
+        entry->retvalue = running;
+        do {
+            thread_yield();
+        } while (!IS_FINISHED(entry));
 
-    while (!IS_FINISHED(entry)) {
-        thread_yield();
+        // Exit from waiting state
+        running->status &= ~WAITING;
     }
 
-    // Exit from waiting state
-    running->status &= ~WAITING;
     // Save returned value if needed
+    TRACE("RETURNING %p IN %p", entry->retvalue, retval);
     if (retval)
         *retval = entry->retvalue;
 
     // Clean up
-    TAILQ_REMOVE(&head, entry, link);
-
     TRACE("DEBUG %p,%d", entry, WAS_ALLOCATED(entry));
     if (WAS_ALLOCATED(entry)) {
         VALGRIND_STACK_DEREGISTER(entry->valgrind_id);
         free(entry->context.uc_stack.ss_sp);
     }
     free(entry);
-    --counter;
-    TRACE("DEBUG %p,%d", running, WAS_ALLOCATED(running));
 
     return 0;
 }
@@ -195,9 +182,14 @@ void thread_exit(void* retval)
     }
 
     running->status |= FINISHED;
+    if (IS_WAITED(running)) {
+        // If the thread was waited by another thread, we need to wake it up.
+        struct context_entry* waited = running->retvalue;
+        TAILQ_INSERT_TAIL(&head, waited, link);
+    }
     running->retvalue = retval;
 
-    if (counter > 1) {
+    if (!TAILQ_EMPTY(&head)) {
         thread_yield();
     }
     exit(0);
@@ -215,6 +207,10 @@ void clear_context(void)
             free(last->context.uc_stack.ss_sp);
             VALGRIND_STACK_DEREGISTER(last->valgrind_id);
         }
+        if (IS_WAITED(last)) {
+            struct context_entry* waited = last->retvalue;
+            TAILQ_INSERT_TAIL(&head, waited, link);
+        }
         free(last);
     }
     VALGRIND_STACK_DEREGISTER(stack_valgrind_id);
@@ -231,9 +227,7 @@ void __attribute__((constructor)) setup_main_thread()
     main->id = main;
     main->status = 0;
     main->retvalue = NULL;
-    TAILQ_INSERT_TAIL(&head, main, link);
     running = main;
-    counter++;
 
     // Create a context with static stack to clean everything at the end.
     getcontext(&context_for_freeing);
@@ -246,6 +240,7 @@ void __attribute__((constructor)) setup_main_thread()
 void __attribute__((destructor)) clear_last_thread()
 {
     TRACE("POST");
+    TAILQ_INSERT_HEAD(&head, running, link);
     // Running is the initial main thread. No need to switch to a static stack.
     if (!WAS_ALLOCATED(running)) {
         clear_context();