diff --git a/Makefile b/Makefile
index f4e9c8a759f89f3eb4e836341b41ffcc04e36b69..f4135c03292eabf6086bc28ea0a2c30d625d56b1 100644
--- a/Makefile
+++ b/Makefile
@@ -3,15 +3,16 @@ CFLAGS = -Iinclude -Wall -g3
 LDFLAGS = -lm
 LIBHQR=src/libhqr.a
 
-default: testing_pivgen testing_treewalk
+default: testing_pivgen testing_treewalk testing_treedraw
 
 src/queue.o:           include/queue.h include/common.h
 src/libhqr.o:          include/common.h include/libhqr.h
 src/libhqr_dbg.o:      include/common.h include/libhqr.h
 src/libhqr_systolic.o: include/common.h include/libhqr.h
 src/treewalk.o:        include/common.h include/libhqr.h include/queue.h
+src/treedraw.o:        include/common.h include/libhqr.h include/libdraw.h
 
-$(LIBHQR): src/libhqr.o src/libhqr_dbg.o src/libhqr_systolic.o src/treewalk.o src/queue.o
+$(LIBHQR): src/libhqr.o src/libhqr_dbg.o src/libhqr_systolic.o src/treewalk.o src/queue.o src/treedraw.o
 	ar cr $@ $^
 
 %.o : %.c
@@ -23,6 +24,9 @@ testing_pivgen : testings/testing_pivgen.o ${LIBHQR}
 testing_treewalk : testings/testing_treewalk.c ${LIBHQR}
 	$(CC) -o $@ $^ $(CFLAGS) $(LDFLAGS)
 
+testing_treedraw : testings/testing_treedraw.o ${LIBHQR}
+	$(CC) -o $@ $^ $(CFLAGS) $(LDFLAGS)
+
 clean :
 	rm -f include/*~
 	rm -f src/*.o src/*~
@@ -30,4 +34,4 @@ clean :
 
 cleanall: clean
 	rm -f ${LIBHQR}
-	rm -f testing_pivgen testing_treewalk
+	rm -f testing_pivgen testing_treewalk testing_treedraw tree.svg
diff --git a/include/libdraw.h b/include/libdraw.h
new file mode 100644
index 0000000000000000000000000000000000000000..e245f75905b8d8092b257e65df6e20db3910db31
--- /dev/null
+++ b/include/libdraw.h
@@ -0,0 +1,42 @@
+/**
+  *
+  * @file libdraw.h
+  *
+  * Header file for all the functions of drawing.
+  *
+  * @copyright 2017 Bordeaux INP, CNRS (LaBRI UMR 5800), Inria,
+  *                      Univ. Bordeaux. All rights reserved.
+  *
+  * @version 1.0.0
+  * @author Raphael Boucherie
+  * @author Mathieu Faverge
+  * @date 2017-04-04
+  *
+  **/
+
+#ifndef _LIBDRAW_H_
+#define _LIBDRAW_H_
+
+#include <common.h>
+
+BEGIN_C_DECLS
+
+/*
+ * Clobal array for color
+ */
+
+extern char *color[4];
+
+/*
+ * function for treedraw
+ */
+
+void libhqr_writeheader(FILE *tree);
+void libhqr_writeend(FILE *tree);
+void libhqr_drawTT(int x, int y, int w, int h, int k, FILE *tree);
+void libhqr_drawTS(int x, int y, int w, int h, int k, FILE *tree);
+void libhqr_drawline(int x1, int y1, int x2, int y2, FILE *tree);
+
+END_C_DECLS
+
+#endif /* _LIBDRAW_H_ */
diff --git a/include/queue.h b/include/queue.h
index 1af84b4d3a2e6e9cac2f1cf6569be89928909cdd..7581a5da8eaefa81bcd59208e1f5cc3c31531c27 100644
--- a/include/queue.h
+++ b/include/queue.h
@@ -20,20 +20,17 @@
 
 BEGIN_C_DECLS
 
-typedef struct libhqr_queue_tile_s{
+typedef struct libhqr_queue_tile_s {
     struct libhqr_queue_tile_s *prev;
     struct libhqr_queue_tile_s *next;
     int numero;
 } libhqr_queue_tile_t;
 
 libhqr_queue_tile_t *libhqr_queue_tile_new (void);
-void libhqr_queue_tile_post  (libhqr_queue_tile_t ** queue_tile, int numero);
-int  libhqr_queue_tile_get   (libhqr_queue_tile_t ** queue_tile);
+void libhqr_queue_tile_push  (libhqr_queue_tile_t ** queue_tile, int numero);
+int  libhqr_queue_tile_head  (libhqr_queue_tile_t ** queue_tile);
+int  libhqr_queue_tile_pop   (libhqr_queue_tile_t ** queue_tile);
 void libhqr_queue_tile_delete(libhqr_queue_tile_t ** queue_tile);
-void libhqr_queue_tile_first (libhqr_queue_tile_t ** queue_tile);
-void libhqr_queue_tile_prev  (libhqr_queue_tile_t ** queue_tile);
-void libhqr_queue_tile_last  (libhqr_queue_tile_t ** queue_tile);
-void libhqr_queue_tile_next  (libhqr_queue_tile_t ** queue_tile);
 
 END_C_DECLS
 
diff --git a/src/queue.c b/src/queue.c
index e0ce90521153bc993c960d2e85255d2cfebc3867..6afbbf7ee9f3965e0e9a6a2fe6c509b93edb33ae 100644
--- a/src/queue.c
+++ b/src/queue.c
@@ -16,6 +16,7 @@
 #include "queue.h"
 #include <stdlib.h>
 #include <stdio.h>
+#include <assert.h>
 
 /****************************************************
  *   Generic functions for queue
@@ -26,48 +27,14 @@ libhqr_queue_tile_t *libhqr_queue_tile_new (void)
    return (NULL);
 }
 
-void libhqr_queue_tile_first (libhqr_queue_tile_t ** queue_tile)
-{
-   if (queue_tile != NULL && *queue_tile != NULL)
-   {
-      while ((*queue_tile)->prev != NULL)
-         libhqr_queue_tile_prev (queue_tile);
-   }
-   return;
-}
-
-void libhqr_queue_tile_prev (libhqr_queue_tile_t ** queue_tile)
-{
-   if (queue_tile != NULL && *queue_tile != NULL)
-      *queue_tile = (*queue_tile)->prev;
-   return;
-}
-
-void libhqr_queue_tile_last (libhqr_queue_tile_t ** queue_tile)
-{
-   if (queue_tile != NULL && *queue_tile != NULL)
-   {
-      while ((*queue_tile)->next != NULL)
-         libhqr_queue_tile_next (queue_tile);
-   }
-   return;
-}
-
-void libhqr_queue_tile_next (libhqr_queue_tile_t ** queue_tile)
-{
-   if (queue_tile != NULL && *queue_tile != NULL)
-      *queue_tile = (*queue_tile)->next;
-   return;
-}
-
-void libhqr_queue_tile_post (libhqr_queue_tile_t ** queue_tile, int numero)
+void libhqr_queue_tile_push (libhqr_queue_tile_t ** queue_tile, int numero)
 {
    if (queue_tile != NULL)
    {
       libhqr_queue_tile_t *p_l = NULL;
       libhqr_queue_tile_t *p_p = NULL;
 
-      libhqr_queue_tile_first (queue_tile);
+      assert( (*queue_tile) == NULL || (*queue_tile)->prev == NULL );
       p_l = *queue_tile;
       p_p = malloc (sizeof (*p_p));
       if (p_p != NULL)
@@ -88,35 +55,31 @@ void libhqr_queue_tile_post (libhqr_queue_tile_t ** queue_tile, int numero)
    return;
 }
 
-int libhqr_queue_tile_get (libhqr_queue_tile_t ** queue_tile)
+int libhqr_queue_tile_head (libhqr_queue_tile_t ** queue_tile)
 {
-   int ret;
-
    if (queue_tile != NULL && *queue_tile != NULL)
    {
-      libhqr_queue_tile_t *p_l = NULL;
-      libhqr_queue_tile_t *p_p = NULL;
-
-      libhqr_queue_tile_last (queue_tile);
-      p_l = *queue_tile;
-      if (p_l != NULL)
-         p_p = p_l->prev;
-      ret = p_l->numero;
-      free (p_l);
-      p_l = NULL;
-      if (p_p != NULL)
-         p_p->next = NULL;
-      *queue_tile = p_p;
+       return (*queue_tile)->numero;
    }
-   return (ret);
+   return -1;
 }
 
-void libhqr_queue_tile_delete (libhqr_queue_tile_t ** queue_tile)
+int libhqr_queue_tile_pop (libhqr_queue_tile_t ** queue_tile)
 {
-   if (queue_tile != NULL && *queue_tile != NULL)
-   {
-      while (*queue_tile != NULL)
-         libhqr_queue_tile_get (queue_tile);
-   }
-   return;
+    int ret = -1;
+    if (queue_tile != NULL)
+    {
+        libhqr_queue_tile_t *p_l = NULL;
+        libhqr_queue_tile_t *p_p = NULL;
+
+        p_l = *queue_tile;
+        p_p = p_l->next;
+        if (p_p != NULL)
+            p_p->prev = NULL;
+        ret = p_l->numero;
+        free (p_l);
+        *queue_tile = p_p;
+    }
+    return (ret);
 }
+
diff --git a/src/treedraw.c b/src/treedraw.c
new file mode 100644
index 0000000000000000000000000000000000000000..99b2b55686e05e195ae6672489f3d1735d6d21a9
--- /dev/null
+++ b/src/treedraw.c
@@ -0,0 +1,68 @@
+/**
+ *
+ * @file treedraw.c
+ *
+ * All the functions required for drawing tree are here.
+ *
+ * @copyright 2017 Bordeaux INP, CNRS (LaBRI UMR 5800), Inria,
+ *                 Univ. Bordeaux. All rights reserved.
+ *
+ * @version 1.0.0
+ * @author Raphael Boucherie
+ * @author Mathieu Faverge
+ * @date 2017-04-04
+ *
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include "libdraw.h"
+
+/*
+ * Global array for color
+ */
+
+char *color[4] = {"red", "blue", "green", "purple"};
+
+/*
+ * functions writing in the svg file
+ */
+
+void libhqr_writeheader(FILE *tree){
+    if(fprintf(tree, "<?xml version=\"1.0\" standalone=\"no\"?>\n"
+                     "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\" \n \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n"
+                     "<svg width=\"2000\" height=\"2000\" version=\"1.1\" \n xmlns=\"http://www.w3.org/2000/svg\">\n") <0)
+        return;
+}
+
+/*
+ * Common prameters to the 2 following functions:
+ *    x - Parameter x for the x-axis
+ *    y - Parameter y for the y-axis
+ *    w - Parameter w for the width
+ *    h - Parameter h for the height
+ *    k - Factorization step for the color
+ */
+
+void libhqr_drawTT(int x, int y, int w, int h, int k, FILE *tree){
+    if(fprintf(tree,"<rect x=\"%d\" y=\"%d\" width=\"%d\" height=\"%d\" fill=\"%s\" /> \n", x, y, w, h, color[k%4]) < 0 )
+        return;
+}
+
+void libhqr_drawTS(int x, int y, int w, int h, int k, FILE *tree){
+    if(fprintf(tree,"<rect x=\"%d\" y=\"%d\" width=\"%d\" height=\"%d\" fill=\"%s\" /> \n", x, y, w, h, color[k%4]) < 0 )
+        return;
+    int x2 = x + (w / 4);
+    int y2 = y + (h / 4);
+    int w2 = (w / 2);
+    int h2 = (h / 2);
+    if(fprintf(tree,"<rect x=\"%d\" y=\"%d\" width=\"%d\" height=\"%d\" fill =\"white\"/> \n", x2, y2, w2, h2) < 0 ) return;
+}
+
+void libhqr_drawline(int x1, int y1, int x2, int y2, FILE *tree){
+    if(fprintf(tree,"<line x1=\"%d\" y1=\"%d\" x2=\"%d\" y2=\"%d\" style=\"fill:none;stroke:black;stroke-width:2px;\"/> \n", x1, y1, x2, y2) < 0 ) return;
+}
+
+void libhqr_writeend(FILE *tree){
+    if(fprintf(tree, "</svg>") < 0) return;
+}
diff --git a/src/treewalk.c b/src/treewalk.c
index 0fbd5f26e978ba6af94d5fe513b31829b84c90c7..d5f11c20316aaab629b98fe1c08aca53bae55984 100644
--- a/src/treewalk.c
+++ b/src/treewalk.c
@@ -16,6 +16,7 @@
 #include "libhqr.h"
 #include "queue.h"
 #include <stdio.h>
+#include <assert.h>
 
 /****************************************************
  *   LIBHQR_TREEWALK
@@ -31,35 +32,50 @@
  *         Factorization step
  *
  */
+void libhqr_treewalk(const libhqr_tree_t *qrtree, int k)
+{
+    int tsid = -1, ttid = -1;
+    int p = k;
+    int pivot = k;
+    libhqr_queue_tile_t *tt = libhqr_queue_tile_new();
+    libhqr_queue_tile_t *ts = libhqr_queue_tile_new();
 
-void libhqr_treewalk(const libhqr_tree_t *qrtree,int k){
-  int pivot = qrtree->mt;
-  printf("%d\n" , pivot);
-  int p = pivot;
-  printf("%d\n" , qrtree->prevpiv(qrtree, k, pivot, p));
-  int a,b;
-  libhqr_queue_tile_t *tt = libhqr_queue_tile_new();
-  libhqr_queue_tile_t *ts = libhqr_queue_tile_new();
-  while(p = qrtree->prevpiv(qrtree, k, pivot, p)){
-      while(p = qrtree->nextpiv(qrtree, k, pivot, p)){
-          if(qrtree->gettype(qrtree, k, p)){
-              libhqr_queue_tile_post(&tt,p);
-          }
-          libhqr_queue_tile_post(&ts, p);
-      }
-      libhqr_queue_tile_last(&ts);
-      libhqr_queue_tile_last(&tt);
-      a = ts->numero;
-      printf("%d\n" , a);
-      b = tt->numero;
-      printf("%d\n" , b);
-      while(a != b){
-          libhqr_queue_tile_get(&ts);
-          libhqr_queue_tile_last(&ts);
-          a = ts->numero;
-          printf("%d\n" , a);
-      }
-      libhqr_queue_tile_last(&tt);
-      libhqr_queue_tile_get(&tt);
-  }
+    libhqr_queue_tile_push(&tt, k);
+
+    while( tt )
+    {
+        /* Stack all elements killed on the way down */
+        p = qrtree->prevpiv(qrtree, k, pivot, p);
+        while( p != qrtree->mt ) {
+            /* If it's a TS tile, or a TT at the bottom of the tree that kills noone */
+            if( (qrtree->gettype(qrtree, k, p) == 0) ||
+                (qrtree->prevpiv(qrtree, k, p, p) != qrtree->mt) )
+            {
+                libhqr_queue_tile_push(&tt,p);
+                printf("PUSH TT: %d\n", p);
+            }
+            libhqr_queue_tile_push(&ts, p);
+            printf("PUSH TS: %d\n", p);
+            p = qrtree->prevpiv(qrtree, k, pivot, p);
+            assert( p != -1 );
+        }
+
+        tsid = libhqr_queue_tile_head(&ts);
+        ttid = libhqr_queue_tile_pop(&tt);
+
+        /* Unstack all element killed by TS, or by TT and already discovered */
+        while(tsid != -1 && tsid != ttid) {
+            printf( "TS=%d, TT=%d\n", tsid, ttid );
+
+            tsid = libhqr_queue_tile_pop(&ts);
+            printf("POP TS: %d\n", tsid);
+            printf("Call function on (%d, %d)\n",
+                   qrtree->currpiv(qrtree, k, tsid), tsid );
+            tsid = libhqr_queue_tile_head(&ts);
+        }
+        pivot = p = ttid;
+    }
+
+    assert(ts == NULL);
+    assert(tt == NULL);
 }
diff --git a/testings/testing_treedraw.c b/testings/testing_treedraw.c
new file mode 100644
index 0000000000000000000000000000000000000000..773d23aa23217ce86e6daaa59be3fc9050565e6b
--- /dev/null
+++ b/testings/testing_treedraw.c
@@ -0,0 +1,41 @@
+/**
+ *
+ * @file testting_treedraw.c
+ *
+ * Testing file for drawing functions and drawing tree
+ *
+ * @copyright 2017 Bordeaux INP, CNRS (LaBRI UMR 5800), Inria,
+ *                      Univ. Bordeaux. All rights reserved.
+ *
+ * @version 1.0.0
+ * @author Raphael Boucherie
+ * @author Mathieu Faverge
+ * @date 2017-04-04
+ *
+ */
+
+
+#include <stdio.h>
+#include <string.h>
+#include "libdraw.h"
+
+#define WIDTH  50
+#define HEIGHT 50
+
+int
+main(int argc, char ** argv)
+{
+  FILE *tree = fopen("tree.svg","w+");
+  int x,y,i,j;
+  libhqr_writeheader(tree);
+  libhqr_drawline(125,100,125,800,tree);
+  for (i = 1; i < 5; i++){
+      for (j = 1; j < 9; j++){
+          x = 100*i;
+          y = 100*j;
+          libhqr_drawTS(x,y,WIDTH,HEIGHT,3,tree);
+      }
+  }
+  libhqr_writeend(tree);
+  return 1;
+}
diff --git a/testings/testing_treewalk.c b/testings/testing_treewalk.c
index af457479ef8de2af2f9000c573d36fa559822dc2..c57056eddcdacf8645479a21e752181982780f3a 100644
--- a/testings/testing_treewalk.c
+++ b/testings/testing_treewalk.c
@@ -23,20 +23,19 @@ main(int argc, char ** argv)
     libhqr_tree_t qrtree;
     libhqr_tiledesc_t matrix;
     int minMN, k, rc ;
-
     /*
      * Test treewalk for HQR trees
      */
     matrix.nodes = 1;
-    matrix.p = 1;
-    matrix.mt = 4;
-    matrix.nt = 13;
-    libhqr_hqr_init( &qrtree, LIBHQR_QR, &matrix, -1 , -1 , -1 , -1, 0, 0);
+    matrix.p     = 1;
+    matrix.mt    = 8;
+    matrix.nt    = 4;
+    libhqr_hqr_init( &qrtree, LIBHQR_QR, &matrix, LIBHQR_FIBONACCI_TREE, LIBHQR_FLAT_TREE, 1, 1, 0, 0);
     minMN = libhqr_imin(matrix.mt, matrix.nt );
     rc = libhqr_tree_check( &matrix, &qrtree );
     printf("%d\n", rc);
     for (k=0; k<minMN; k++) {
-        libhqr_treewalk( &qrtree, k);
+        libhqr_treewalk( &qrtree, k );
     }
     libhqr_hqr_finalize( &qrtree );
 }