include/Base_Packet.h

@@ -9,125 +9,385 @@
 #define INCLUDE_BASEPACKET_H_
 
 #include <vector>
-#include <map>
+#include <unordered_map>
 #include <string>
 #include <cstring>
 #include <algorithm>
+#include <numeric>
 #include <iostream>
 #include <cmath>
+#include <functional>
+#include <optional>
 
 namespace inaf::oasbo::PacketLib {
 
 class BasePacketStructure {
-
-public:
-	virtual ~BasePacketStructure() = default;
-	virtual std::vector<std::tuple<int, std::string, int>> getPacketStructure() = 0;
-};
-
-class _iterator;
-class BasePacket {
 protected:
+	std::string source;
+	size_t byteSize;
 	std::vector<int> fieldSizes;
-	std::map<size_t, size_t> fieldNameToOffsetsMap;
-	std::map<std::string, int> fieldNameToIndexMap;
-	std::map<int, std::string> indexToFieldNameMap;
-	uint8_t *binaryPointer;
-
+	std::unordered_map<size_t, size_t> fieldNameToOffsetsMap;
+	std::unordered_map<std::string, int> fieldNameToIndexMap;
+	std::unordered_map<int, std::string> indexToFieldNameMap;
 
+	virtual std::vector<std::tuple<int, std::string, int>> readStructureFromSource(
+			std::string source) = 0;
 	void updateFieldSizes(
 			const std::vector<std::tuple<int, std::string, int>> &paramsTuple) {
 		std::for_each(paramsTuple.begin(), paramsTuple.end(),
 				[&](const std::tuple<int, std::string, int> &tup) {
-					this->fieldSizes.push_back(std::get < 2 > (tup));
+					this->fieldSizes.push_back(std::get<2>(tup));
 				});
 	}
-
 	void updateFieldOffsets(
 			const std::vector<std::tuple<int, std::string, int>> &paramsTuple) {
 		size_t offset = 0;
 		for (size_t i = 0; i < paramsTuple.size(); i++) {
 			fieldNameToOffsetsMap[i] = offset;
-			offset += std::get < 2 > (paramsTuple[i]);
+			offset += std::get<2>(paramsTuple[i]);
 		}
 	}
 	void updateFieldNameAndIndexMap(
 			const std::vector<std::tuple<int, std::string, int>> &paramsTuple) {
 		std::for_each(paramsTuple.begin(), paramsTuple.end(),
 				[&](const std::tuple<int, std::string, int> &tup) {
-					this->fieldNameToIndexMap[std::get < 1 > (tup)] = std::get
-							< 0 > (tup);
-					this->indexToFieldNameMap[std::get < 0 > (tup)] = std::get
-							< 1 > (tup);
+					this->fieldNameToIndexMap[std::get<1>(tup)] = std::get<0>(
+							tup);
+					this->indexToFieldNameMap[std::get<0>(tup)] = std::get<1>(
+							tup);
 				});
 	}
+	void updateStructure(std::string source) {
+		this->source = source;
+		std::vector<std::tuple<int, std::string, int>> paramsTuple =
+				readStructureFromSource(source);
+		updateFieldSizes(paramsTuple);
+		updateFieldOffsets(paramsTuple);
+		updateFieldNameAndIndexMap(paramsTuple);
+		this->byteSize = std::accumulate(fieldSizes.begin(), fieldSizes.end(),
+				0) / 8 + 1; // /8 +1 for byte size
+	}
 
 public:
+	virtual ~BasePacketStructure() = default;
+	void changeSource(std::string source) {
+		updateStructure(source);
+	}
+	size_t getByteSize() {
+		return this->byteSize;
+	}
+	std::string getSource() {
+		return this->source;
+	}
+	std::optional<size_t> bitOffsetOf(uint index) {
+		if (index <= this->numberOfFields()) {
+			return fieldNameToOffsetsMap.at(index);
+		} else {
+			std::cerr << "No field at " << index << ", max is "
+					<< numberOfFields() << ", returning nullopt." << std::endl;
+			return std::nullopt;
+		}
+	}
+	std::optional<size_t> bitSizeOf(uint index) {
+		if (index <= this->numberOfFields()) {
+			return this->fieldSizes[index];
+		} else {
+			std::cerr << "No field at " << index << ", max is "
+					<< numberOfFields() << ", returning nullopt." << std::endl;
+			return std::nullopt;
+		}
+	}
+
+	std::optional<size_t> indexOfField(std::string fieldName) {
+		try {
+			return this->fieldNameToIndexMap.at(fieldName);
+		} catch (const std::out_of_range &oor) {
+			std::cerr << "No field of name " << fieldName
+					<< ", returning nullopt." << std::endl;
+			return std::nullopt;
+		}
+	}
 
-	BasePacket(std::vector<std::tuple<int, std::string, int>> paramsTuple) {
-		this->updateFieldSizes(paramsTuple);
-		this->updateFieldNameAndIndexMap(paramsTuple);
-		this->updateFieldOffsets(paramsTuple);
-		this->binaryPointer = new uint8_t[paramsTuple.size() * sizeof(size_t)]; //tot number of field times the maximum bytesize of the value;
+	std::optional<std::string> fieldNameOfIndex(size_t index) {
+		try {
+			return this->indexToFieldNameMap.at(index);
+		} catch (const std::out_of_range &oor) {
+			std::cerr << "No field at " << index << ", max is "
+					<< numberOfFields() << ", returning nullopt." << std::endl;
+			return std::nullopt;
+		}
+	}
+	size_t numberOfFields() {
+		return this->fieldSizes.size();
+	}
+
+	std::unordered_map<std::string, int> getFieldNameToIndexMap() const {
+		return this->fieldNameToIndexMap;
+	}
+};
+
+template<typename ValueType>
+class bit_iterator: public std::iterator<std::random_access_iterator_tag,
+		ValueType> {
+public:
+	bit_iterator(const uint8_t *data, int offset,
+			BasePacketStructure *structure,
+			std::function<ValueType(const uint8_t*, size_t, int)> func) :
+			m_data(data), m_offset(offset), m_structure(structure), m_func(func) {
+	}
+
+	bit_iterator(const bit_iterator &other) :
+			m_data(other.m_data), m_offset(other.m_offset), m_structure(
+					other.m_structure), m_func(other.m_func) {
+	}
+
+	bit_iterator& operator++() {
+		++m_offset;
+		return *this;
+	}
+
+	bit_iterator operator++(int) {
+		bit_iterator temp(*this);
+		++m_offset;
+		return temp;
+	}
+
+	bit_iterator& operator--() {
+		--m_offset;
+		return *this;
+	}
+
+	bit_iterator operator--(int) {
+		bit_iterator temp(*this);
+		--m_offset;
+		return temp;
+	}
+
+	bit_iterator operator+(int n) const {
+		return bit_iterator(m_data, m_offset + n, m_structure, m_func);
+	}
+
+	bit_iterator operator-(int n) const {
+		return bit_iterator(m_data, m_offset - n, m_structure, m_func);
+	}
+
+	int operator-(const bit_iterator &other) const {
+		return m_offset - other.m_offset;
+	}
+
+	bool operator==(const bit_iterator &other) const {
+		return m_data == other.m_data && m_offset == other.m_offset;
 	}
-	virtual ~BasePacket() = default;
 
-	size_t readValueFromBinaryAt(int index) {
-		size_t num_bit_read = fieldNameToOffsetsMap[index]; // offset from the beginning of the byte
-		int bit_rem = this->fieldSizes[index]; //remaining bits to read
-		size_t value = 0; //final value of the field
-		while (bit_rem > 0) {
-			size_t read = this->binaryPointer[num_bit_read / 8];
-			size_t mask = std::pow(2, 8 - num_bit_read % 8) - 1;
-			read = read & mask;
-
-			bit_rem = bit_rem - 8 + num_bit_read % 8;
-			read = (bit_rem >= 0) * (read << bit_rem)
-					+ (bit_rem < 0) * (read >> -bit_rem);
-			value += read;
-
-			num_bit_read = (bit_rem > 0) * (num_bit_read + 8 - num_bit_read % 8)
-					+ (bit_rem <= 0)
-							* (num_bit_read + 8 - num_bit_read % 8 + bit_rem);
+	bool operator!=(const bit_iterator &other) const {
+		return !(*this == other);
+	}
+
+	ValueType operator*() const {
+		auto offset = m_structure->bitOffsetOf(m_offset); // offset from the beginning of the byte
+		auto num_bits = m_structure->bitSizeOf(m_offset);
+		return m_func(m_data, offset.value(), num_bits.value());
+	}
+
+private:
+	const uint8_t *m_data;
+	int m_offset;
+	BasePacketStructure *m_structure;
+	std::function<ValueType(const uint8_t*, size_t, int)> m_func;
+};
+
+template<typename ValueType>
+class BasePacketTempl {
+protected:
+	BasePacketStructure *structure;
+	uint8_t *binaryPointer;
+
+	// This C++ function reads a binary value from a memory location pointed to by binaryPointer.
+	// The binary value is represented by num_bits number of bits starting from the offset-th bit in the memory.
+	// The function returns the value of the binary as a ValueType.
+	static ValueType _readValueFromBinaryAt_(const uint8_t *binaryPointer,
+			size_t offset, int num_bits) {
+		// Calculate the bit offset from the byte offset:
+		int bit_offset = offset % 8;
+		// Calculate the byte offset from the bit offset:
+		int byte_offset = offset / 8;
+		ValueType value = 0;
+
+		for (int i = 0; i < num_bits; i++) {
+			// Calculate the byte and bit index of the current bit:
+			int byte_index = byte_offset + (bit_offset + i) / 8;
+			int bit_index = (bit_offset + i) % 8;
+			uint8_t byte = binaryPointer[byte_index];
+			// Create a bit mask to isolate the desired bit:
+			uint8_t bit_mask = 1 << (7 - bit_index);
+			// Set the corresponding bit in the return value if the retrieved bit is 1:
+			value |= (byte & bit_mask) ? (1 << (num_bits - i - 1)) : 0;
 		}
 		return value;
 	}
 
-	std::vector<int> const* getFieldSizes() const {
-		return &fieldSizes;
+	int minBitsRequired(size_t value) {
+		// Handle special cases
+		if (value == 0) {
+			return 1;
+		} else if (value == (size_t) -1) {
+			return sizeof(value) * 8;
+		}
+
+		// Calculate the number of bits needed
+		int bitsNeeded = 0;
+		bool isNegative = value < 0;
+		uint32_t absValue = isNegative ? -value : value;
+
+		while (absValue != 0) {
+			bitsNeeded++;
+			absValue >>= 1;
+		}
+
+		if (isNegative) {
+			bitsNeeded++;
+		}
+
+		return bitsNeeded;
+	}
+public:
+
+	BasePacketTempl(BasePacketStructure &structure) {
+		this->structure = &structure;
+		this->binaryPointer = new uint8_t[structure.getByteSize()];
+	}
+
+	virtual ~BasePacketTempl() = default;
+
+	void updateStructure(BasePacketStructure &structure) {
+		this->structure = &structure;
 	}
+
+	std::optional<ValueType> readValueFromBinaryAt(uint index) {
+		auto offset = structure->bitOffsetOf(index); // offset from the beginning of the byte
+		auto num_bits = structure->bitSizeOf(index); //remaining bits to read
+		if (offset.has_value() && num_bits.has_value())
+			return _readValueFromBinaryAt_(binaryPointer, offset.value(),
+					num_bits.value());
+		else {
+			std::cerr << "Error: No field at " << index << ", max is "
+					<< structure->numberOfFields() << ", returning nullopt"
+					<< std::endl;
+			return std::nullopt;
+		}
+	}
+
 	uint8_t const* getBinaryPointer() const {
 		return binaryPointer;
 	}
 
 	void copyToBinaryPointer(const uint8_t *from, uint size) {
-		std::memcpy(binaryPointer, from, size);
+		if (size > this->structure->getByteSize()) {
+			std::cerr << "Error: you are trying to copy " << size
+					<< "byte where the max size is: "
+					<< this->structure->getByteSize() << std::endl;
+			std::cerr << "\tI copy only until "
+					<< this->structure->getByteSize() << " byte" << std::endl;
+			std::memcpy(binaryPointer, from, this->structure->getByteSize());
+		} else
+			std::memcpy(binaryPointer, from, size);
 	}
 	void copyToBinaryPointer(const uint8_t *from, uint size, uint offset) {
-		std::memcpy(&binaryPointer[offset], from, size);
+		if (size + offset > this->structure->getByteSize()) {
+			std::cerr << "Error: you are trying to copy " << size + offset
+					<< "byte where the max size is: "
+					<< this->structure->getByteSize() << std::endl;
+			std::cerr << "\tI copy only until "
+					<< this->structure->getByteSize() << " byte" << std::endl;
+			std::memcpy(&binaryPointer[offset], from,
+					this->structure->getByteSize());
+		} else
+			std::memcpy(&binaryPointer[offset], from, size);
 	}
 
-	size_t indexOfField(std::string fieldName) {
-		return this->fieldNameToIndexMap.at(fieldName);
-	}
-	std::string fieldNameOfIndex(size_t index) {
-		return this->indexToFieldNameMap.at(index);
+	std::optional<ValueType> operator[](uint index) {
+		return readValueFromBinaryAt(index);
 	}
 
-	size_t operator[](int index)  {
-		return readValueFromBinaryAt(index);
+	std::optional<ValueType> operator[](std::string fieldName) {
+		auto index = structure->indexOfField(fieldName);
+		if (index.has_value())
+			return readValueFromBinaryAt(index.value());
+		else {
+			std::cerr << "Error: no field with name '" << fieldName
+					<< "', returning 0" << std::endl;
+			return std::nullopt;
+		}
+	}
 
+	bit_iterator<ValueType> begin() const {
+		return bit_iterator<ValueType>(binaryPointer, 0, structure,
+				&_readValueFromBinaryAt_);
 	}
 
-	size_t  operator[](std::string fieldName) {
-		int index = this->fieldNameToIndexMap.at(fieldName);
-		return readValueFromBinaryAt(index);
+	bit_iterator<ValueType> end() const {
+		return bit_iterator<ValueType>(binaryPointer,
+				structure->numberOfFields(), structure,
+				&_readValueFromBinaryAt_);
 	}
 
+	int writeValueToBinaryAtIndex(size_t index, ValueType value) {
+		try {
+			size_t offset = this->structure->bitOffsetOf(index);
+			size_t numbits = this->structure->bitSizeOf(index);
+			size_t min_req = minBitsRequired(value);
+			if (numbits < min_req) {
+				std::cerr << "Error: you are trying to write " << value
+						<< " which requires at least " << min_req
+						<< " bits in a field of size " << numbits << std::endl;
+				return -1;
+			}
+
+			// Calculate the bit offset from the byte offset:
+			int bitoffset = offset % 8;
+			// Calculate the byte offset from the bit offset:
+			int byteoffset = offset / 8;
+			int numbits_written = 0;
+
+			for (size_t i = 0; i < numbits; i++) {
+				// Calculate the byte and bit index of the current bit:
+				int byte_index = byteoffset + (bitoffset + i) / 8;
+				int bit_index = (bitoffset + i) % 8;
+				// Create a bit mask to isolate the desired bit:
+				uint8_t bit_mask = 1 << (7 - bit_index);
+				// Set the corresponding bit in the binary array if the value is 1:
+				if ((value >> (numbits - i - 1)) & 1) {
+					binaryPointer[byte_index] |= bit_mask;
+					numbits_written++;
+				} else {
+					binaryPointer[byte_index] &= ~bit_mask;
+				}
+			}
+			return numbits_written;
+		} catch (const std::out_of_range &oor) {
+			std::cerr << "Error: no field at index '" << index
+					<< ", returning -1" << std::endl;
+			return -1;
+		}
+
+	}
 	virtual size_t getHeaderSize()=0;
 	virtual size_t getPayloadSize()=0;
 	virtual size_t getTailSize()=0;
 	virtual size_t getMaxPacketSize()=0;
 };
+
+using valueType = size_t;
+class BasePacket: public BasePacketTempl<valueType> {
+
+protected:
+
+public:
+	BasePacket(BasePacketStructure &structure) :
+			BasePacketTempl<valueType>(structure) {
+	}
+	virtual ~BasePacket() = default;
+
+};
 }
 #endif