1 files changed, 66 insertions, 27 deletions

diff --git a/std/queue.zc b/std/queue.zc
index 3e99eba..0b00ee2 100644
--- a/std/queue.zc
+++ b/std/queue.zc
@@ -3,13 +3,15 @@ import "./mem.zc"
 
 struct Queue<T> {
   data: T*;
-  len: usize;
   cap: usize;
+  head: usize;
+  tail: usize;
+  count: usize;
 }
 
 impl Queue<T> {
   fn new() -> Queue<T> {
-    return Queue<T>{data: NULL, len: 0, cap: 0};
+    return Queue<T>{data: NULL, cap: 0, head: 0, tail: 0, count: 0};
   }
 
   fn free(self) {
@@ -17,54 +19,91 @@ impl Queue<T> {
       free(self.data);
       self.data = NULL;
     }
-    self.len = 0;
+    self.cap = 0;
+    self.head = 0;
+    self.tail = 0;
+    self.count = 0;
+  }
+
+  fn _grow(self) {
+    var new_cap = (self.cap == 0) ? 8 : self.cap * 2;
+    var new_data = malloc(sizeof(T) * new_cap);
+    
+    if (self.count > 0) {
+        if (self.tail > self.head) {
+            memcpy(new_data, self.data + self.head, sizeof(T) * self.count);
+        } else {
+            var first_part = self.cap - self.head;
+            var second_part = self.tail;
+            memcpy(new_data, self.data + self.head, sizeof(T) * first_part);
+            memcpy(new_data + first_part, self.data, sizeof(T) * second_part);
+        }
+    }
+    
+    if (self.data) free(self.data);
+    self.data = new_data;
+    self.cap = new_cap;
+    self.head = 0;
+    self.tail = self.count;
   }
 
   fn clone(self) -> Queue<T> {
     var new_queue = Queue<T>::new();
-    new_queue.len = self.len;
+    new_queue.data = malloc(sizeof(T) * self.cap);
     new_queue.cap = self.cap;
-    new_queue.data = malloc(sizeof(T) * new_queue.cap);
-    memcpy(new_queue.data, self.data, sizeof(T) * new_queue.len);
+    new_queue.head = 0;
+    new_queue.tail = self.count;
+    new_queue.count = self.count;
+    
+    if (self.count > 0) {
+        if (self.tail > self.head) {
+            memcpy(new_queue.data, self.data + self.head, sizeof(T) * self.count);
+        } 
+        else {
+            var first_part = self.cap - self.head;
+            var second_part = self.tail;
+            memcpy(new_queue.data, self.data + self.head, sizeof(T) * first_part);
+            memcpy(new_queue.data + first_part, self.data, sizeof(T) * second_part);
+        }
+    }
+    
     return new_queue;
   }
 
   fn push(self, value: T) {
-    if (!self.data) {
-      self.cap = 8;
-      self.data = malloc(sizeof(T) * self.cap);
+    if (self.count == self.cap) {
+      self._grow();
     }
-    if (self.len == self.cap) {
-      self.cap = self.cap * 2;
-      self.data = realloc(self.data, sizeof(T) * self.cap);
-    }
-    self.data[self.len] = value;
-    self.len = self.len + 1;
+    
+    self.data[self.tail] = value;
+    self.tail = (self.tail + 1) % self.cap;
+    self.count = self.count + 1;
   }
 
   fn pop(self) -> Option<T> {
-    if (self.len > 0) {
-      var value = self.data[0];
-      self.len = self.len - 1;
-
-      // Shifts the data in the queue "forward"
-      memcpy(self.data, self.data + 1, sizeof(T) * self.len);
-
-      return Option<T>::Some(value);
+    if (self.count == 0) {
+      return Option<T>::None();
     }
-    return Option<T>::None();
+    
+    var value = self.data[self.head];
+    self.head = (self.head + 1) % self.cap;
+    self.count = self.count - 1;
+    
+    return Option<T>::Some(value);
   }
 
   fn length(self) -> usize {
-    return self.len;
+    return self.count;
   }
 
   fn clear(self) {
-    self.len = 0;
+    self.head = 0;
+    self.tail = 0;
+    self.count = 0;
   }
 
   fn is_empty(self) -> bool {
-    return self.len == 0;
+    return self.count == 0;
   }
 }