From e81a883bda68f3ec204a3cb731d9e116987ce7e6 Mon Sep 17 00:00:00 2001
From: Benjamin Kraft <benjamin.kraft@online.de>
Date: Sat, 28 Jan 2023 14:11:41 +0100
Subject: [PATCH] queue fix

---
 src/Queue.h | 19 +++++++------------
 1 file changed, 7 insertions(+), 12 deletions(-)

diff --git a/src/Queue.h b/src/Queue.h
index 7af3a49..17bb331 100644
--- a/src/Queue.h
+++ b/src/Queue.h
@@ -23,23 +23,15 @@ class Queue {
 		}
 	};
 	virtual void update() {}
-
-	// First in
+	// First in: points to next free slot
 	size_t tail = 0;
-
-	// First out
+	// First out: points to leaving element position
 	size_t head = 0;
-
 	T * container[MAX_QUEUE_SIZE];
 protected:
 	bool isEmpty() const {
 		return tail == head;
 	}
-	size_t size() const {
-		if (tail > head)
-			return tail - head;
-		return MAX_QUEUE_SIZE - head + tail;
-	}
 	T * pop(){
 		mutex.lock();
 		T * element = container[head];
@@ -53,8 +45,11 @@ public:
 	}
 	void push(T * element){
 		mutex.lock();
-		container[tail] = element;
-		tail = (tail + 1) % MAX_QUEUE_SIZE;
+		if ((tail + 1) % MAX_QUEUE_SIZE != head){
+			container[tail] = element;
+			tail = (tail + 1) % MAX_QUEUE_SIZE;
+		} else
+			std::cout << "Queue is full!\n";
 		mutex.unlock();
 	}
 };