diff --git a/src/queue.c b/src/queue.c
index 8ae3e970f3a41315f7f896f566efaada9da23ff5..66429a4c56e5706aa43eb6c4fb383a22425c0700 100644
--- a/src/queue.c
+++ b/src/queue.c
@@ -103,19 +103,19 @@ void queue_insert ( struct queue *q , struct task *t ) {
     q->count += 1;
     
     /* Shuffle up. */
-    /* for ( int k = q->count - 1 ; k > 0 ; k = (k-1)/2 )
+    for ( int k = q->count - 1 ; k > 0 ; k = (k-1)/2 )
         if ( q->tasks[ q->tid[k] ].weight > q->tasks[ q->tid[(k-1)/2] ].weight ) {
             int temp = q->tid[k];
             q->tid[k] = q->tid[(k-1)/2];
             q->tid[(k-1)/2] = temp;
             }
         else
-            break; */
-    for ( int k = q->count - 1 ; k > 0 && q->tasks[ q->tid[k-1] ].weight < q->tasks[ q->tid[k] ].weight ; k-- ) {
+            break;
+    /* for ( int k = q->count - 1 ; k > 0 && q->tasks[ q->tid[k-1] ].weight < q->tasks[ q->tid[k] ].weight ; k-- ) {
         int temp = q->tid[k];
         q->tid[k] = q->tid[k-1];
         q->tid[k-1] = temp;
-        }
+        } */
             
     /* Verify queue consistency. */
     /* for ( int k = 1 ; k < q->count ; k++ )
@@ -238,7 +238,7 @@ struct task *queue_gettask ( struct queue *q , int qid , int blocking ) {
                 cj->super->owner = qid;
                 
             /* Swap this task with the last task and re-heap. */
-            /* if ( k < qcount ) {
+            if ( k < qcount ) {
                 qtid[ k ] = qtid[ qcount ];
                 while ( qtasks[ qtid[k] ].weight > qtasks[ qtid[(k-1)/2] ].weight ) {
                     int temp = q->tid[k];
@@ -246,6 +246,7 @@ struct task *queue_gettask ( struct queue *q , int qid , int blocking ) {
                     q->tid[(k-1)/2] = temp;
                     k = (k-1)/2;
                     }
+                int i;
                 while ( ( i = 2*k+1 ) < qcount ) {
                     if ( i+1 < qcount && qtasks[ qtid[i+1] ].weight > qtasks[ qtid[i] ].weight )
                         i += 1;
@@ -258,14 +259,14 @@ struct task *queue_gettask ( struct queue *q , int qid , int blocking ) {
                     else
                         break;
                     }
-                } */
-            qtid[ k ] = qtid[ qcount ];
+                }
+            /* qtid[ k ] = qtid[ qcount ];
             while ( k < qcount-1 && qtasks[ qtid[k+1] ].weight > qtasks[ qtid[k] ].weight ) {
                 temp = qtid[k];
                 qtid[k] = qtid[k+1];
                 qtid[k+1] = temp;
                 k += 1;
-                }
+                } */
                 
             /* Verify queue consistency. */
             /* for ( k = 1 ; k < q->count ; k++ )
diff --git a/src/scheduler.c b/src/scheduler.c
index 76c495c7ee6e16830515cb9adc900e76dab9fb8f..468d4f75adcb8e4f34233a82e8b6d251f16b3afb 100644
--- a/src/scheduler.c
+++ b/src/scheduler.c
@@ -563,47 +563,47 @@ void scheduler_start ( struct scheduler *s , unsigned int mask ) {
     // #pragma omp parallel for schedule(static) private(t,j)
     for ( k = s->nr_tasks-1 ; k >= 0 ; k-- ) {
         t = &tasks[ tid[k] ];
-        if ( ( (1 << t->type) & mask ) && !t->skip ) {
-            for ( j = 0 ; j < t->nr_unlock_tasks ; j++ )
-                atomic_inc( &t->unlock_tasks[j]->wait );
-            t->maxdepth = 0;
-            t->weight = 0;
-            for ( j = 0 ; j < t->nr_unlock_tasks ; j++ ) {
-                if ( t->unlock_tasks[j]->weight > t->weight )
-                    t->weight = t->unlock_tasks[j]->weight;
-                if ( t->unlock_tasks[j]->maxdepth > t->maxdepth )
-                    t->maxdepth = t->unlock_tasks[j]->maxdepth;
-                }
-            t->maxdepth += 1;
-            if ( t->tic > 0 )
-                t->weight += t->toc - t->tic;
-            else
-                switch ( t->type ) {
-                    case task_type_sort:
-                        t->weight += t->ci->count * ( sizeof(int)*8 - __builtin_clz( t->ci->count ) );
-                        break;
-                    case task_type_self:
-                        t->weight += t->ci->count * t->ci->count;
-                        break;
-                    case task_type_pair:
+        if ( !( (1 << t->type) & mask ) || !t->skip )
+            continue;
+        for ( j = 0 ; j < t->nr_unlock_tasks ; j++ )
+            atomic_inc( &t->unlock_tasks[j]->wait );
+        t->maxdepth = 0;
+        t->weight = 0;
+        for ( j = 0 ; j < t->nr_unlock_tasks ; j++ ) {
+            if ( t->unlock_tasks[j]->weight > t->weight )
+                t->weight = t->unlock_tasks[j]->weight;
+            if ( t->unlock_tasks[j]->maxdepth > t->maxdepth )
+                t->maxdepth = t->unlock_tasks[j]->maxdepth;
+            }
+        t->maxdepth += 1;
+        if ( t->tic > 0 )
+            t->weight += t->toc - t->tic;
+        else
+            switch ( t->type ) {
+                case task_type_sort:
+                    t->weight += t->ci->count * ( sizeof(int)*8 - __builtin_clz( t->ci->count ) );
+                    break;
+                case task_type_self:
+                    t->weight += t->ci->count * t->ci->count;
+                    break;
+                case task_type_pair:
+                    t->weight += t->ci->count * t->cj->count;
+                    break;
+                case task_type_sub:
+                    if ( t->cj != NULL )
                         t->weight += t->ci->count * t->cj->count;
-                        break;
-                    case task_type_sub:
-                        if ( t->cj != NULL )
-                            t->weight += t->ci->count * t->cj->count;
-                        else
-                            t->weight += t->ci->count * t->ci->count;
-                        break;
-                    case task_type_ghost:
-                        if ( t->ci == t->ci->super )
-                            t->weight += t->ci->count;
-                        break;
-                    case task_type_kick1:
-                    case task_type_kick2:
+                    else
+                        t->weight += t->ci->count * t->ci->count;
+                    break;
+                case task_type_ghost:
+                    if ( t->ci == t->ci->super )
                         t->weight += t->ci->count;
-                        break;
-                    }
-            }
+                    break;
+                case task_type_kick1:
+                case task_type_kick2:
+                    t->weight += t->ci->count;
+                    break;
+                }
         }
         
     /* Loop over the tasks and enqueue whoever is ready. */
@@ -636,7 +636,6 @@ void scheduler_enqueue ( struct scheduler *s , struct task *t ) {
         case task_type_self:
         case task_type_sort:
         case task_type_ghost:
-        case task_type_kick1:
         case task_type_kick2:
             qid = t->ci->super->owner;
             break;