From 88f229cef924fcb45c46b8d6e002c3d8142f60d8 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Thu, 23 Apr 2020 15:03:55 +0200
Subject: [PATCH] fifo enhancement

---
 container/FIFO.h | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/container/FIFO.h b/container/FIFO.h
index 134da9b8..889d2ade 100644
--- a/container/FIFO.h
+++ b/container/FIFO.h
@@ -3,6 +3,11 @@
 
 #include <framework/returnvalues/HasReturnvaluesIF.h>
 
+/**
+ * @brief Simple First-In-First-Out data structure
+ * @tparam T Entry Type
+ * @tparam capacity Maximum capacity
+ */
 template<typename T, uint8_t capacity>
 class FIFO {
 private:
@@ -54,6 +59,26 @@ public:
 			return HasReturnvaluesIF::RETURN_OK;
 		}
 	}
+
+	ReturnValue_t peek(T * value) {
+	    if(empty()) {
+	        return EMPTY;
+	    } else {
+	       *value = data[readIndex];
+	       return HasReturnvaluesIF::RETURN_OK;
+	    }
+	}
+
+	ReturnValue_t pop() {
+	    if(empty()) {
+	        return EMPTY;
+	    } else {
+	        readIndex = next(readIndex);
+	        --currentSize;
+	        return HasReturnvaluesIF::RETURN_OK;
+	    }
+	}
+
 	static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS;
 	static const ReturnValue_t FULL = MAKE_RETURN_CODE(1);
 	static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2);