diff --git a/src/static_queue.c b/src/static_queue.c
index 29c281d..b7f18b5 100644
--- a/src/static_queue.c
+++ b/src/static_queue.c
@@ -135,15 +135,13 @@ int32_t staticQueueErase(staticQueue_t* queue, staticQueueItem_t* item)
     item->active = false;
 
     // Special case: if this was the only item in the queue
-    if (queue->tail == queue->head->last) {
+    if (queue->tail == queue->head->last && queue->tail == item) {
         // Queue is now empty, reset pointers
         queue->head = queue->first_item;
         queue->tail = queue->first_item;
         return STATIC_QUEUE_SUCCESS;
-    }
-
-    // If erasing the tail item (oldest item), just move tail to next
-    if (item == queue->tail) {
+    } else if (item == queue->tail) {
+        // If erasing the tail item (oldest item), just move tail to next
         queue->tail = queue->tail->next;
 
         // Skip over any remaining inactive items at tail
@@ -158,10 +156,8 @@ int32_t staticQueueErase(staticQueue_t* queue, staticQueueItem_t* item)
         }
 
         return STATIC_QUEUE_SUCCESS;
-    }
-
-    // If erasing the item just before head (newest item), move head backward
-    if (item->next == queue->head) {
+    } else if (item->next == queue->head) {
+        // If erasing the item just before head (newest item), move head backward
         queue->head = item;
 
         // Check if head caught up to tail with exactly one active item
@@ -173,7 +169,24 @@ int32_t staticQueueErase(staticQueue_t* queue, staticQueueItem_t* item)
         return STATIC_QUEUE_SUCCESS;
     }
 
-    // For items in the middle: remove from current position and relink immediately before tail
+    // For items in the middle: first verify the item is actually in the queue
+    // We can start from the element one from the tail as we have allready checked the tail item above.
+    staticQueueItem_t* current = queue->tail->next;
+    bool found = false;
+
+    // Traverse from tail to head to find the item
+    while (current != queue->head) {
+        if (current == item) {
+            found = true;
+            break;
+        }
+        current = current->next;
+    }
+
+    // If not found, the item is not in this queue
+    if (!found) {
+        return STATIC_QUEUE_NOT_IN_QUEUE;
+    }
 
     // Step 1: Remove item from its current position in the list
     staticQueueItem_t* prev_item = item->last;
@@ -198,3 +211,87 @@ int32_t staticQueueErase(staticQueue_t* queue, staticQueueItem_t* item)
 
     return STATIC_QUEUE_SUCCESS;
 }
+
+int32_t staticQueueGetNumItems(staticQueue_t* queue)
+{
+    if (queue == NULL) {
+        return STATIC_QUEUE_EMPTY;
+    }
+
+    if (staticQueueEmpty(queue)) {
+        return 0;
+    }
+
+    // Check if queue is full
+    if (staticQueuefull(queue)) {
+        return queue->queue_length;
+    }
+
+    // Count active items from tail to head
+    int32_t count = 0;
+    staticQueueItem_t *current = queue->tail;
+
+    while (current != queue->head) {
+        if (current->active) {
+            count++;
+        }
+        current = current->next;
+    }
+
+    return count;
+}
+
+int32_t staticQueueForEach(staticQueue_t* queue, int32_t (*callback)(staticQueue_t *queue, staticQueueItem_t *item))
+{
+    if (queue == NULL || callback == NULL) {
+        return STATIC_QUEUE_EMPTY;
+    }
+
+    if (staticQueueEmpty(queue)) {
+        return STATIC_QUEUE_SUCCESS;
+    }
+
+    // Get the number of items to process
+    int32_t num_items = staticQueueGetNumItems(queue);
+    if (num_items <= 0) {
+        return STATIC_QUEUE_SUCCESS;
+    }
+
+    staticQueueItem_t *current = queue->tail;
+    int32_t processed = 0;
+
+    // Process exactly num_items active items
+    while (processed < num_items) {
+        // Only process active items
+        if (current->active) {
+            int32_t cb_res = callback(queue, current);
+            switch(cb_res) {
+                case STATIC_QUEUE_CB_NEXT:
+                    current = current->next;
+                    processed++;
+                    break;
+                case STATIC_QUEUE_CB_STOP:
+                    return STATIC_QUEUE_SUCCESS;
+                case STATIC_QUEUE_CB_ERASE: {
+                    staticQueueItem_t *tmp = current;
+                    current = current->next;
+                    processed++;
+                    // Erase this item from the queue
+                    if ((cb_res = staticQueueErase(queue, tmp)) != STATIC_QUEUE_SUCCESS) {
+                        return cb_res;
+                    }
+                    // Check if queue is now empty after erase
+                    if (staticQueueEmpty(queue)) {
+                        return STATIC_QUEUE_SUCCESS;
+                    }
+                } break;
+                default:
+                    return cb_res;
+            }
+        } else {
+            current = current->next;
+        }
+    }
+
+    return STATIC_QUEUE_SUCCESS;
+}
diff --git a/src/static_queue.h b/src/static_queue.h
index 711d8d5..8fdc82d 100644
--- a/src/static_queue.h
+++ b/src/static_queue.h
@@ -79,11 +79,18 @@
 
 // Package queue
 typedef enum {
-    STATIC_QUEUE_SUCCESS = 0,
-    STATIC_QUEUE_FULL    = -401,
-    STATIC_QUEUE_EMPTY   = -402,
+    STATIC_QUEUE_SUCCESS      = 0,
+    STATIC_QUEUE_FULL         = -401,
+    STATIC_QUEUE_EMPTY        = -402,
+    STATIC_QUEUE_NOT_IN_QUEUE = -403,
 } queueErr_t;
 
+typedef enum {
+    STATIC_QUEUE_CB_NEXT  = 0, // Keep iterating
+    STATIC_QUEUE_CB_STOP,      // Stop iterating
+    STATIC_QUEUE_CB_ERASE,     // Erase this node and keep iterating
+} staticQueueCbDo_t;
+
 typedef struct staticQueueItem staticQueueItem_t;
 
 struct staticQueueItem {
@@ -173,6 +180,21 @@ bool staticQueuefull(staticQueue_t* queue);
  */
 bool staticQueueEmpty(staticQueue_t* queue);
 
+/**
+ * Get the number of active items in the queue
+ * Input: Queue instance
+ * Returns: Number of items in queue, or negative error code
+ */
+int32_t staticQueueGetNumItems(staticQueue_t* queue);
+
+/**
+ * Loop through all items in queue and call the callback on each
+ * Input: Queue instance
+ * Input: Callback function
+ * Returns: queueErr_t
+ */
+int32_t staticQueueForEach(staticQueue_t* queue, int32_t (*callback)(staticQueue_t *queue, staticQueueItem_t *item));
+
 /**
  * This is a macro that makes it more safe to initialize a queue
  */
diff --git a/test/test_static_queue.c b/test/test_static_queue.c
index e2d6b6a..a511c4c 100644
--- a/test/test_static_queue.c
+++ b/test/test_static_queue.c
@@ -82,6 +82,72 @@ static int32_t queueClear(staticQueue_t* queue)
 #define THIRD_DATA  1337
 #define FOURTH_DATA 59
 
+// Global counters for ForEach callbacks
+static int32_t g_foreach_counter = 0;
+static int32_t g_foreach_sum = 0;
+
+// ForEach callback functions
+static int32_t countCallback(staticQueue_t *q, staticQueueItem_t *item) {
+    (void)q;  // Unused
+    myList_t* list_item = CONTAINER_OF(item, myList_t, node);
+    g_foreach_counter++;
+    g_foreach_sum += list_item->number;
+    return STATIC_QUEUE_CB_NEXT;
+}
+
+static int32_t stopCallback(staticQueue_t *q, staticQueueItem_t *item) {
+    (void)q;  // Unused
+    (void)item;  // Unused
+    g_foreach_counter++;
+    if (g_foreach_counter >= 2) {
+        return STATIC_QUEUE_CB_STOP;
+    }
+    return STATIC_QUEUE_CB_NEXT;
+}
+
+static int32_t eraseCallback(staticQueue_t *q, staticQueueItem_t *item) {
+    (void)q;  // Unused
+    myList_t* list_item = CONTAINER_OF(item, myList_t, node);
+    if (list_item->number > 25) {
+        return STATIC_QUEUE_CB_ERASE;
+    }
+    return STATIC_QUEUE_CB_NEXT;
+}
+
+static int32_t eraseAllCallback(staticQueue_t *q, staticQueueItem_t *item) {
+    (void)q;  // Unused
+    (void)item;  // Unused
+    return STATIC_QUEUE_CB_ERASE;
+}
+
+static int32_t eraseEvenCallback(staticQueue_t *q, staticQueueItem_t *item) {
+    (void)q;  // Unused
+    myList_t* list_item = CONTAINER_OF(item, myList_t, node);
+    if (list_item->number % 2 == 0) {
+        return STATIC_QUEUE_CB_ERASE;
+    }
+    return STATIC_QUEUE_CB_NEXT;
+}
+
+static int32_t errorCallback(staticQueue_t *q, staticQueueItem_t *item) {
+    (void)q;  // Unused
+    (void)item;  // Unused
+    g_foreach_counter++;
+    if (g_foreach_counter == 3) {
+        return -999;  // Custom error
+    }
+    return STATIC_QUEUE_CB_NEXT;
+}
+
+static int32_t eraseOnly20Callback(staticQueue_t *q, staticQueueItem_t *item) {
+    (void)q;  // Unused
+    myList_t* list_item = CONTAINER_OF(item, myList_t, node);
+    if (list_item->number == 20) {
+        return STATIC_QUEUE_CB_ERASE;
+    }
+    return STATIC_QUEUE_CB_NEXT;
+}
+
 int main() {
 
     staticQueue_t queue;
@@ -1236,6 +1302,400 @@ int main() {
 
     printf("\n=== All extended tests passed ===\n");
 
+    // ===== Test staticQueueGetNumItems function =====
+    printf("\n=== Testing staticQueueGetNumItems ===\n");
+
+    // Test: Empty queue
+    printf("\nTest: GetNumItems on empty queue\n");
+    queueClear(&queue);
+    int32_t num = staticQueueGetNumItems(&queue);
+    if (num != 0) {
+        printf("Expected 0 items in empty queue, got %i\n", num);
+        return 1;
+    }
+    printf("Passed: Empty queue has 0 items\n");
+
+    // Test: Single item
+    printf("\nTest: GetNumItems with single item\n");
+    queuePut(&queue, 10);
+    num = staticQueueGetNumItems(&queue);
+    if (num != 1) {
+        printf("Expected 1 item, got %i\n", num);
+        return 1;
+    }
+    printf("Passed: Single item counted correctly\n");
+
+    // Test: Multiple items (not full)
+    printf("\nTest: GetNumItems with multiple items\n");
+    queueClear(&queue);
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    num = staticQueueGetNumItems(&queue);
+    if (num != 3) {
+        printf("Expected 3 items, got %i\n", num);
+        return 1;
+    }
+    printf("Passed: Multiple items counted correctly\n");
+
+    // Test: Full queue
+    printf("\nTest: GetNumItems on full queue\n");
+    queueClear(&queue);
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    queuePut(&queue, 40);
+    if (!staticQueuefull(&queue)) {
+        printf("Queue should be full\n");
+        return 1;
+    }
+    num = staticQueueGetNumItems(&queue);
+    if (num != LIST_LEN) {
+        printf("Expected %i items in full queue, got %i\n", LIST_LEN, num);
+        return 1;
+    }
+    printf("Passed: Full queue counted correctly (%i items)\n", LIST_LEN);
+
+    // Test: After pop
+    printf("\nTest: GetNumItems after pop\n");
+    queueClear(&queue);
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    queuePop(&queue, &data);
+    num = staticQueueGetNumItems(&queue);
+    if (num != 2) {
+        printf("Expected 2 items after pop, got %i\n", num);
+        return 1;
+    }
+    printf("Passed: Count correct after pop\n");
+
+    // Test: After erase
+    printf("\nTest: GetNumItems after erase\n");
+    queueClear(&queue);
+    staticQueueItem_t* erase_items[3];
+    staticQueuePut(&queue, &erase_items[0]);
+    staticQueuePut(&queue, &erase_items[1]);
+    staticQueuePut(&queue, &erase_items[2]);
+
+    staticQueueErase(&queue, erase_items[1]);  // Erase middle item
+    num = staticQueueGetNumItems(&queue);
+    if (num != 2) {
+        printf("Expected 2 items after erase, got %i\n", num);
+        return 1;
+    }
+    printf("Passed: Count correct after erase\n");
+
+    printf("\n=== All staticQueueGetNumItems tests passed ===\n");
+
+    // ===== Test staticQueueForEach function =====
+    printf("\n=== Testing staticQueueForEach ===\n");
+
+    // Test 20: Basic iteration - count all items
+    printf("\nTest 20: ForEach - basic iteration\n");
+    queueClear(&queue);
+    g_foreach_counter = 0;
+    g_foreach_sum = 0;
+
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    queuePut(&queue, 40);
+
+    result = staticQueueForEach(&queue, countCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach failed: %i\n", result);
+        return 1;
+    }
+
+    if (g_foreach_counter != 4) {
+        printf("Expected to iterate 4 items, got %i\n", g_foreach_counter);
+        return 1;
+    }
+
+    if (g_foreach_sum != 100) {
+        printf("Expected sum 100, got %i\n", g_foreach_sum);
+        return 1;
+    }
+
+    printf("Test 20 passed: ForEach iterated all items correctly\n");
+
+    // Test 21: ForEach on empty queue
+    printf("\nTest 21: ForEach on empty queue\n");
+    queueClear(&queue);
+    g_foreach_counter = 0;
+
+    result = staticQueueForEach(&queue, countCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach on empty queue failed: %i\n", result);
+        return 1;
+    }
+
+    if (g_foreach_counter != 0) {
+        printf("Expected 0 iterations on empty queue, got %i\n", g_foreach_counter);
+        return 1;
+    }
+
+    printf("Test 21 passed: ForEach handles empty queue\n");
+
+    // Test 22: ForEach with STOP
+    printf("\nTest 22: ForEach with STOP\n");
+    queueClear(&queue);
+    g_foreach_counter = 0;
+
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    queuePut(&queue, 40);
+
+    result = staticQueueForEach(&queue, stopCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach stop failed: %i\n", result);
+        return 1;
+    }
+
+    if (g_foreach_counter != 2) {
+        printf("Expected to stop after 2 items, got %i\n", g_foreach_counter);
+        return 1;
+    }
+
+    printf("Test 22 passed: ForEach stops correctly\n");
+
+    // Test 23: ForEach with ERASE
+    printf("\nTest 23: ForEach with ERASE\n");
+    queueClear(&queue);
+
+    queuePut(&queue, 10);
+    queuePut(&queue, 30);
+    queuePut(&queue, 20);
+    queuePut(&queue, 40);
+
+    result = staticQueueForEach(&queue, eraseCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach erase failed: %i\n", result);
+        return 1;
+    }
+
+    // Should only have 10 and 20 left
+    result = queuePop(&queue, &data);
+    if (result != STATIC_QUEUE_SUCCESS || data != 10) {
+        printf("Expected 10, got %u\n", data);
+        return 1;
+    }
+
+    result = queuePop(&queue, &data);
+    if (result != STATIC_QUEUE_SUCCESS || data != 20) {
+        printf("Expected 20, got %u\n", data);
+        return 1;
+    }
+
+    if (!staticQueueEmpty(&queue)) {
+        printf("Queue should be empty after erasing 30 and 40\n");
+        return 1;
+    }
+
+    printf("Test 23 passed: ForEach erases items correctly\n");
+
+    // Test 24: ForEach erase all items
+    printf("\nTest 24: ForEach erase all items\n");
+    queueClear(&queue);
+
+    queuePut(&queue, 100);
+    queuePut(&queue, 200);
+    queuePut(&queue, 300);
+
+    result = staticQueueForEach(&queue, eraseAllCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach erase all failed: %i\n", result);
+        return 1;
+    }
+
+    if (!staticQueueEmpty(&queue)) {
+        printf("Queue should be empty after erasing all\n");
+        return 1;
+    }
+
+    printf("Test 24 passed: ForEach can erase all items\n");
+
+    // Test 25: ForEach with single item
+    printf("\nTest 25: ForEach with single item\n");
+    queueClear(&queue);
+    g_foreach_counter = 0;
+
+    queuePut(&queue, 999);
+
+    result = staticQueueForEach(&queue, countCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach single item failed: %i\n", result);
+        return 1;
+    }
+
+    if (g_foreach_counter != 1) {
+        printf("Expected 1 iteration, got %i\n", g_foreach_counter);
+        return 1;
+    }
+
+    printf("Test 25 passed: ForEach handles single item\n");
+
+    // Test 26: ForEach after pop operations
+    printf("\nTest 26: ForEach after pop operations\n");
+    queueClear(&queue);
+    g_foreach_counter = 0;
+
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    queuePut(&queue, 40);
+
+    // Pop first two items
+    queuePop(&queue, &data);
+    queuePop(&queue, &data);
+
+    result = staticQueueForEach(&queue, countCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach after pop failed: %i\n", result);
+        return 1;
+    }
+
+    if (g_foreach_counter != 2) {
+        printf("Expected 2 items after popping 2, got %i\n", g_foreach_counter);
+        return 1;
+    }
+
+    printf("Test 26 passed: ForEach works after pop operations\n");
+
+    // Test 27: ForEach selective erase
+    printf("\nTest 27: ForEach selective erase (even numbers)\n");
+    queueClear(&queue);
+
+    queuePut(&queue, 1);
+    queuePut(&queue, 2);
+    queuePut(&queue, 3);
+    queuePut(&queue, 4);
+
+    result = staticQueueForEach(&queue, eraseEvenCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach selective erase failed: %i\n", result);
+        return 1;
+    }
+
+    // Should have 1 and 3 left
+    result = queuePop(&queue, &data);
+    if (result != STATIC_QUEUE_SUCCESS || data != 1) {
+        printf("Expected 1, got %u\n", data);
+        return 1;
+    }
+
+    result = queuePop(&queue, &data);
+    if (result != STATIC_QUEUE_SUCCESS || data != 3) {
+        printf("Expected 3, got %u\n", data);
+        return 1;
+    }
+
+    if (!staticQueueEmpty(&queue)) {
+        printf("Queue should be empty\n");
+        return 1;
+    }
+
+    printf("Test 27 passed: ForEach selectively erases items\n");
+
+    // Test 28: ForEach with NULL callback
+    printf("\nTest 28: ForEach with NULL callback (error handling)\n");
+    queueClear(&queue);
+    queuePut(&queue, 10);
+
+    result = staticQueueForEach(&queue, NULL);
+    if (result != STATIC_QUEUE_EMPTY) {
+        printf("Expected error with NULL callback, got %i\n", result);
+        return 1;
+    }
+
+    printf("Test 28 passed: ForEach handles NULL callback\n");
+
+    // Test 29: ForEach callback returning error
+    printf("\nTest 29: ForEach callback returning error\n");
+    queueClear(&queue);
+    g_foreach_counter = 0;
+
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    queuePut(&queue, 40);
+
+    result = staticQueueForEach(&queue, errorCallback);
+    if (result != -999) {
+        printf("Expected callback error -999, got %i\n", result);
+        return 1;
+    }
+
+    if (g_foreach_counter != 3) {
+        printf("Expected to process 3 items before error, got %i\n", g_foreach_counter);
+        return 1;
+    }
+
+    printf("Test 29 passed: ForEach propagates callback errors\n");
+
+    // Test 30: Erase single item in a queue with one item
+    printf("\nTest 30: ForEach erase single item from single-item queue\n");
+    queueClear(&queue);
+    queuePut(&queue, 999);
+
+    result = staticQueueForEach(&queue, eraseAllCallback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach failed: %i\n", result);
+        return 1;
+    }
+
+    if (!staticQueueEmpty(&queue)) {
+        printf("Queue should be empty after erasing single item\n");
+        return 1;
+    }
+
+    printf("Test 30 passed: Single item erased from single-item queue\n");
+
+    // Test 31: Erase ONE element in middle, keep the rest
+    printf("\nTest 31: ForEach erase only middle element, keep others\n");
+    queueClear(&queue);
+
+    queuePut(&queue, 10);
+    queuePut(&queue, 20);
+    queuePut(&queue, 30);
+    queuePut(&queue, 40);
+
+    result = staticQueueForEach(&queue, eraseOnly20Callback);
+    if (result != STATIC_QUEUE_SUCCESS) {
+        printf("ForEach failed: %i\n", result);
+        return 1;
+    }
+
+    // Should have 10, 30, 40 left (not 20)
+    result = queuePop(&queue, &data);
+    if (result != STATIC_QUEUE_SUCCESS || data != 10) {
+        printf("Expected 10, got %u\n", data);
+        return 1;
+    }
+
+    result = queuePop(&queue, &data);
+    if (result != STATIC_QUEUE_SUCCESS || data != 30) {
+        printf("Expected 30, got %u\n", data);
+        return 1;
+    }
+
+    result = queuePop(&queue, &data);
+    if (result != STATIC_QUEUE_SUCCESS || data != 40) {
+        printf("Expected 40, got %u\n", data);
+        return 1;
+    }
+
+    if (!staticQueueEmpty(&queue)) {
+        printf("Queue should be empty\n");
+        return 1;
+    }
+
+    printf("Test 31 passed: Only one middle element erased, others kept\n");
+
+    printf("\n=== All staticQueueForEach tests passed ===\n");
+
     // Connect first driver and app
     printf("\nTest Done\n");
 }
\ No newline at end of file