From 90e1aa223d34aaa4a47643c6fed230ee253c4c41 Mon Sep 17 00:00:00 2001
From: Kurt Wachowski <kwachowski@intrepidcs.com>
Date: Thu, 12 Oct 2023 17:33:05 +0000
Subject: [PATCH] Packetizer:

---
 communication/packetizer.cpp              | 11 ++-
 include/icsneo/communication/packetizer.h | 95 ++++++++++++++++++++++-
 2 files changed, 99 insertions(+), 7 deletions(-)

diff --git a/communication/packetizer.cpp b/communication/packetizer.cpp
index 18cf28b..a09108f 100644
--- a/communication/packetizer.cpp
+++ b/communication/packetizer.cpp
@@ -26,7 +26,8 @@ std::vector<uint8_t>& Packetizer::packetWrap(std::vector<uint8_t>& data, bool sh
 
 bool Packetizer::input(const std::vector<uint8_t>& inputBytes) {
 	bool haveEnoughData = true;
-	bytes.insert(bytes.end(), inputBytes.begin(), inputBytes.end());
+
+	bytes.Copy(inputBytes);
 
 	while(haveEnoughData) {
 		switch(state) {
@@ -151,16 +152,14 @@ bool Packetizer::input(const std::vector<uint8_t>& inputBytes) {
 				if(packetLength > 0)
 					packet.data.resize(packetLength - headerSize);
 
-				auto i = 0;
-				while(currentIndex < packetLength)
-					packet.data[i++] = bytes[currentIndex++];
+				bytes.CopyTo(packet.data.data(), currentIndex, (packetLength - currentIndex));
+				currentIndex = packetLength;
 
 				if(disableChecksum || !checksum || bytes[currentIndex] == ICSChecksum(packet.data)) {
 					// Got a good packet
 					gotGoodPackets = true;
 					processedPackets.push_back(std::make_shared<Packet>(packet));
-					for (auto a = 0; a < packetLength; a++)
-						bytes.pop_front();
+					bytes.Erase_front(packetLength);
 
 					if(packet.network == Network::NetID::DiskData && (packetLength - headerSize) % 2 == 0) {
 						bytes.pop_front();
diff --git a/include/icsneo/communication/packetizer.h b/include/icsneo/communication/packetizer.h
index 7895ce3..cfee43b 100644
--- a/include/icsneo/communication/packetizer.h
+++ b/include/icsneo/communication/packetizer.h
@@ -8,6 +8,9 @@
 #include <queue>
 #include <vector>
 #include <memory>
+#include <cstring>
+
+#define ICSNEO_PACKETIZER_BUFFER_SIZE (512 * 1024)
 
 namespace icsneo {
 
@@ -33,6 +36,96 @@ private:
 		ParseDiskDataHeader,
 		GetData
 	};
+
+	class RingBuffer
+	{
+	private:
+		constexpr static size_t mBufferSize = ICSNEO_PACKETIZER_BUFFER_SIZE;
+
+		size_t mStartOffset;
+		size_t mSize;
+		uint8_t mData[mBufferSize];
+
+	public:
+		RingBuffer(void)
+			: mStartOffset(0)
+			, mSize(0)
+		{
+			(void)memset(mData, 0, mBufferSize);
+		}
+
+		const uint8_t& operator [](size_t offset) { return Get(offset); }
+		size_t size(void) { return mSize; }
+		void pop_front(void)
+		{
+			Erase_front(1);
+		}
+
+		void Erase_front(size_t count)
+		{
+			if (mSize < count)
+			{
+				throw std::runtime_error("RingBuffer: Underflow");
+			}
+			mStartOffset = (mStartOffset + count) % mBufferSize;
+			mSize -= count;
+		}
+
+		const uint8_t& Get(size_t offset)
+		{
+			if (offset >= mSize)
+			{
+				throw std::runtime_error("RingBuffer: Index out of range");
+			}
+			return *Resolve(offset);
+		}
+
+		void Copy(const std::vector<uint8_t>& source)
+		{
+			const auto inputSize = source.size();
+			const auto octetsAvailable = (mBufferSize - mSize);
+
+			if (inputSize > octetsAvailable)
+			{
+				throw std::runtime_error("RingBuffer: Out of memory");
+			}
+
+			const auto octetsAvailableTail = (octetsAvailable - mStartOffset);
+			const auto octetsToWrap = (inputSize > octetsAvailableTail) ? (inputSize - octetsAvailableTail) : 0;
+			const auto octetsToAppend = (inputSize - octetsToWrap);
+
+			(void)memcpy(Resolve(mSize), source.data(), octetsToAppend);
+			if (octetsToWrap > 0)
+			{
+				(void)memcpy(mData, &source.data()[octetsToAppend], octetsToWrap);
+			}
+			mSize += inputSize;
+		}
+
+		void CopyTo(uint8_t* dest, size_t startIndex, size_t length)
+		{
+			if ((startIndex + length) > mSize)
+			{
+				throw std::runtime_error("RingBuffer: Index out of range");
+			}
+
+			const auto octetsToReadHead = std::min<size_t>((mBufferSize - mStartOffset - startIndex), length);
+			const auto octetsToReadTail = (length - octetsToReadHead);
+
+			(void)memcpy(dest, Resolve(startIndex), octetsToReadHead);
+			if (octetsToReadTail > 0)
+			{
+				(void)memcpy(&dest[octetsToReadHead], mData, octetsToReadTail);
+			}
+		}
+
+	protected:
+		inline uint8_t* Resolve(size_t offset)
+		{
+			return &mData[(mStartOffset + offset) % mBufferSize];
+		}
+	};
+
 	ReadState state = ReadState::SearchForHeader;
 
 	int currentIndex = 0;
@@ -41,7 +134,7 @@ private:
 	bool checksum = false;
 	bool gotGoodPackets = false; // Tracks whether we've ever gotten a good packet
 	Packet packet;
-	std::deque<uint8_t> bytes;
+	RingBuffer bytes;
 
 	std::vector<std::shared_ptr<Packet>> processedPackets;