queue fix

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();
 	}
 };