Include/Fledasty/Core/Queue.h

@@ -39,5 +39,6 @@ FledastyError fledasty_queue_initialize(FledastyQueue *new_queue, const size_t e
 FledastyError fledasty_queue_destroy(FledastyQueue *current_queue);
 
 FledastyError fledasty_queue_push(FledastyQueue *current_queue, void *value);
+void *fledasty_queue_peek(FledastyQueue *current_queue);
 
 #endif

Src/Core/Queue.c

@@ -66,17 +66,34 @@ FledastyError fledasty_queue_push(FledastyQueue *current_queue, void *value) {
         return FLEDASTY_ERROR_INVALID_POINTER;
     }
 
-    if (current_queue->capacity <= current_queue->size + 1) {
+    if (current_queue->size == current_queue->capacity) {
         current_queue->capacity = current_queue->capacity + (current_queue->capacity / 2);
         current_queue->buffer = (unsigned char*)hallocy_realloc(current_queue->buffer, current_queue->capacity * current_queue->element_byte_size);
 
         if (current_queue->buffer == NULL) {
             return FLEDASTY_ERROR_FAILED_ALLOCATION;
         }
+
+        size_t new_tail = current_queue->capacity - current_queue->tail;
+        hallocy_move_memory(current_queue->buffer + (current_queue->tail * current_queue->element_byte_size), current_queue->buffer + (new_tail * current_queue->element_byte_size), current_queue->size - current_queue->tail);
+        current_queue->tail = new_tail;
     }
 
-    hallocy_copy_memory(current_queue->buffer + (current_queue->size * current_queue->element_byte_size), value, current_queue->element_byte_size);
+    hallocy_copy_memory(current_queue->buffer + (current_queue->head * current_queue->element_byte_size), value, current_queue->element_byte_size);
 
     current_queue->size += 1;
+    current_queue->head += 1;
+    if (current_queue->head >= current_queue->capacity) {
+        current_queue->head = 0;
+    }
+    
     return FLEDASTY_ERROR_NONE;
 }
+
+void *fledasty_queue_peek(FledastyQueue *current_queue) {
+    if (current_queue == NULL) {
+        return NULL;
+    }
+
+    return current_queue->buffer + (current_queue->tail * current_queue->element_byte_size);
+}

Tests/Main.c

@@ -30,6 +30,9 @@ int main() {
         fledasty_queue_push(&test_queue, &i);
     }
 
+    int *peek_data = (int*)fledasty_queue_peek(&test_queue);
+    printf("Peeked: %d\n", *peek_data);
+
     fledasty_queue_destroy(&test_queue);
 
     printf("Done\n");